%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% VPE Solver. Copyright @ Xingqi Zhang
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Ex_all, Ey_all] = Func_VPE(f, eps_r, sigma, distance, TX, k_w0, shape, BC, cross_section, dx, ds)
%   This function simulates electromagnetic wave propagation inside a 
%   tunnel using the Vector Parabolic Equation (VPE) method.
%
%   OUTPUTS:
%       Ex_all : Electric field distribution (x-polarization) inside tunnel
%       Ey_all : Electric field distribution (y-polarization) inside tunnel
%
%   INPUTS:
%       f            : Operating frequency [Hz]
%       eps_r        : Relative permittivity of tunnel wall material
%       sigma        : Conductivity of tunnel wall material [S/m]
%       distance     : Total simulation distance along the tunnel axis [m]
%       TX           : Transmitter specifications
%                      TX{1} : Horizontal offset of Tx from tunnel center [m]
%                      (positive = right side, negative = left side)
%                      TX{2} : Vertical height of Tx above tunnel floor [m]
%                      TX{3} : Transmitter antenna gain
%                      TX{4} : Transmitter EIRP [dBm]
%       k_w0         : Beam waist parameter (Beam waist w0 = k_w0 * lambda)
%       shape        : Tunnel cross-section shape type
%                      1 ==> Rectangular
%                      2 ==> Arch
%                      3 ==> Horse-shoe
%                      4 ==> Trapezoidal
%       BC           : Boundary condition type for tunnel walls
%                      1 ==> Impedance boundary condition
%                      2 ==> PEC
%                      3 ==> PMC
%       cross_section: Cross-section geometry parameters
%                      Depending on ‘shape’, fields are:
%                      If shape = 1 (Rectangle):
%                          cross_section{1} = width
%                          cross_section{2} = height
%                      If shape = 2 or 3 (Arch / Horse-shoe):
%                          cross_section{1} = half-width
%                          cross_section{2} = H_top (top height or radius)
%                          cross_section{3} = R2_bottom (bottom radius)
%                          cross_section{4} = H_bottom (bottom height)
%                      If shape = 4 (Trapezoidal):
%                          cross_section{1} = half-width
%                          cross_section{2} = H_top
%                          cross_section{3} = R2_bottom
%                          cross_section{4} = H_bottom
%                          cross_section{5} = x_B1 (breakpoint x-offset)
%                          cross_section{6} = y_B2 (breakpoint y-height)
%       dx           : Grid step size across tunnel cross-section [m]
%       ds           : Step size along tunnel axis [m]

%%% Input parameters
c = 299792458.0;
lambda = c/f;
k0 = 2*pi/lambda;
mu_0 = 4*pi*1e-7;
eps_0 = 1/mu_0/c/c;
sigma_r = sigma/(2*pi*f*eps_0);
eps_rc = eps_r-1j*sigma_r;
Z = sqrt(eps_rc-1)/eps_rc;
s_start = 0;
s_end = distance;

%%%------------------------------------------------------------------------
%%% Geometry parameters
if shape == 1
    H_top = cross_section{2}/2;
    W_half = cross_section{1}/2;
    H_bottom = H_top;
    R2_bottom = 0;
    shape_top = 4;                % 1.circular or elliptic; 2.parabolic; 3.trapezoid; 4.rectangular; 5.self-defined (For all cases, left and right sides need to be symmetric)
    shape_bottom = 4;             % 1.circular or elliptic; 2.parabolic; 3.trapezoid; 4.rectangular; 5.self-defined (For all cases, left and right sides need to be symmetric)

elseif shape == 2
    H_top = cross_section{2};
    W_half = cross_section{1};
    H_bottom = cross_section{4};
    R2_bottom = cross_section{3};
    shape_top = 1;                % 1.circular or elliptic; 2.parabolic; 3.trapezoid; 4.rectangular; 5.self-defined (For all cases, left and right sides need to be symmetric)
    shape_bottom = 1;             % 1.circular or elliptic; 2.parabolic; 3.trapezoid; 4.rectangular; 5.self-defined (For all cases, left and right sides need to be symmetric)

elseif shape == 3
    H_top = cross_section{2};
    W_half = cross_section{1};
    H_bottom = cross_section{4};
    R2_bottom = cross_section{3};
    shape_top = 1;                % 1.circular or elliptic; 2.parabolic; 3.trapezoid; 4.rectangular; 5.self-defined (For all cases, left and right sides need to be symmetric)
    shape_bottom = 4;             % 1.circular or elliptic; 2.parabolic; 3.trapezoid; 4.rectangular; 5.self-defined (For all cases, left and right sides need to be symmetric)

elseif shape == 4
    H_top = cross_section{2};
    W_half = cross_section{1};
    H_bottom = cross_section{4};
    R2_bottom = cross_section{3};	
    x_B1 = cross_section{5};	
    y_B2 = cross_section{6};	
    shape_top = 5;                % 1.circular or elliptic; 2.parabolic; 3.trapezoid; 4.rectangular; 5.self-defined (For all cases, left and right sides need to be symmetric)
    shape_bottom = 5;             % 1.circular or elliptic; 2.parabolic; 3.trapezoid; 4.rectangular; 5.self-defined (For all cases, left and right sides need to be symmetric)

end


%%%------------------------------------------------------------------------
%%% Mesh discretization
dy = dx;                                % cellsize along vertical direction

nh_top = ceil(V_roundn(H_top/dy));      % number of cells along the top part, veritically
nh_bottom = ceil(V_roundn(H_bottom/dy));% number of cells from center to the bottom (platform), veritically
nw_half = ceil(V_roundn(W_half/dx));    % number of cells for half width part, horizontally
Nx = 2*nw_half;                         % number of cells along the horizontal direction
Ny = nh_top + nh_bottom;                % number of cells along the vertical direction
x_c = Nx/2 + 1;
y_c = nh_top + 1;

Nz = ceil((s_end-s_start)/ds);          % number of cells along the propagation direction

switch shape_top
    case 1
        %%% 1 Circular or elliptic
        %%% Parameters needed if top shape is circular or elliptic
        %%% Points needed for calculation: (W_half,0) and (0,H_top)
           %%% x^2/R1^2 + (y+Hs_top)^2/R2^2 = 1
           %%% Note, in this way, the diameter do not have to be normal to the circle
           Hs_top = 0;                                    % vertical shift only for circular or elliptic. Used for the case that top is not a full semi-circle or semi-ellipse
           X_t = 0;                                       % x coordinate at the top corner, X_t = 0 if top is full semi-arc; (X_t always >= 0)
           R2_top = H_top + Hs_top;                       % circular or elliptic, along y axis
           R1_top = sqrt(W_half^2/(1-(Hs_top/R2_top)^2)); % circular or elliptic, along x axis
    case 2
        %%% 2 Parabolic: y = p1*x^2 + p2
        %%% Parameters needed if top shape is parabolic
        %%% Points needed for calculation: (W_half,0) and (0,H_top)
           X_t = 0;                              % x coordinate at the top corner, X_t = 0 if top is full semi-parabola; (X_t always >= 0)
           p2_top = H_top;                       % parabolic
           p1_top = -p2_top/W_half/W_half;       % parabolic
    case 3
        %%% 3 Trapezoid: y = t1*x + t2
        %%% Parameters needed if top shape is trapezoid
        %%% Points needed for calculation: (W_half,0) and (x_t,H_top)
        X_t = x_B1;                              % x coordinate at the top corner
        t1_top = H_top/(X_t-W_half);             % trapezoid
        t2_top = -t1_top*W_half;                 % trapezoid
    case 5
        %%% 5 Self-defined multiple planar facets: y_B = k_B*x_B + c_B
        %%% Note sequence of points are from top to bottom
        %%% Note points are defined on the right side, left and right are symmetric; x_B_t and y_B_t are relative to the center axis
        x_B_t = [0; x_B1; W_half; W_half];         % x coordinate of breakings points
        y_B_t = [H_top; H_top; y_B2-H_bottom; 0];            % y coordinate of breakings points
        n_points_t = length(x_B_t);                % number of breaking points
        n_seg_t = n_points_t - 1;                  % number of segments
end

switch shape_bottom
    case 1
        %%% 1 Circular or elliptic
        %%% Parameters needed if bottom shape is circular or elliptic
        %%% Points needed for calculation: (W_half,0) and (x_b,-H_bottom)
           %%% x^2/R1^2 + (y-Hs_bottom)^2/R2^2 = 1
           %%% Note, in this way, the diameter do not have to be normal to the circle
           Hs_bottom = 0;                        % vertical shift only for circular or elliptic. Used for the case that bottom is not a full semi-circle or semi-ellipse
%          X_b = 2.3;                           % x coordinate at the bottom corner, if elliptic; (X_b always >= 0)
           X_b = sqrt(W_half^2-H_bottom^2);      % x coordinate at the bottom corner, if circular; (X_b always >= 0)
           % X_b = 0;                            % x coordinate at the bottom corner, X_b = 0 if bottom is full semi-arc;  (X_b always >= 0)
           R1_bottom = sqrt( W_half^2 + Hs_bottom^2*(W_half^2-X_b^2)/((H_bottom^2+2*H_bottom*Hs_bottom)) ); % circular or elliptic, along x axis
           R2_bottom = sqrt( ((H_bottom^2+2*H_bottom*Hs_bottom)*R1_bottom^2)/(W_half^2-X_b^2) );            % circular or elliptic, along y axis
    case 2
        %%% 2 Parabolic: y = p1*x^2 + p2
        %%% Parameters needed if bottom shape is parabolic
        %%% Points needed for calculation: (W_half,0) and (x_b,-H_bottom)
           X_b = 1.75;                            % x coordinate at the bottom corner, X_b = 0 if bottom is full semi-arc; (X_b always >= 0)
           p1_bottom = -H_bottom/(X_b^2-W_half^2);% parabolic
           p2_bottom = -p1_bottom*W_half^2;       % parabolic
    case 3
        %%% 3 Trapezoid: y = t1*x + t2
        %%% Parameters needed if bottom shape is trapezoid
        %%% Points needed for calculation: (W_half,0) and (x_b,-H_bottom)
        X_b = x_B1;                               % x coordinate at the bottom corner
        t1_bottom = -H_bottom/(X_b-W_half);      % trapezoid
        t2_bottom = -t1_bottom*W_half;           % trapezoid
    case 5
        %%% 5 Self-defined multiple planar facets: y_B = k_B*x_B + c_B
        %%% Note sequence of points are from top to bottom
        %%% Note points are defined on the right side, left and right are symmetric; x_B_b and y_B_b are relative to the center axis
        x_B_b =    [W_half; W_half; 0];          % x coordinate of breakings points
        y_B_b = - [0; H_bottom; H_bottom];         % y coordinate of breakings points
        n_points_b = length(x_B_b);              % number of breaking points
        n_seg_b = n_points_b - 1;                % number of segments
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%------------------------------------------------------------------------
%%% coefficients
a_x = Inf(Ny+1,Nx+1);
R_x1 = zeros(Ny+1,Nx+1);
R_x2 = zeros(Ny+1,Nx+1);
R_x3 = zeros(Ny+1,Nx+1);
R_x2_c = zeros(Ny+1,Nx+1);
R_x2_n = zeros(Ny+1,Nx+1);
R_in_d = zeros(Ny+1,Nx+1);
Z_x1 = zeros(Ny+1,Nx+1);
Z_x2 = zeros(Ny+1,Nx+1);
Z_x3 = zeros(Ny+1,Nx+1);
Z_x4 = zeros(Ny+1,Nx+1);

%%% Top part matrix -------------------------------------------------------
%%% Note yy_t did not include the boundary part
yy_t = (nh_top-1:-1:0)*dy;                       % increase yy_t, to calculate the length (residue) for x direction
%%% Pick the geometry shape for the top area
switch shape_top
    case 1                                       % circular or elliptic
        xx_t = R1_top*sqrt(1-(yy_t+Hs_top).^2/R2_top^2);
    case 2                                       % parabolic
        xx_t = sqrt((yy_t-p2_top)./p1_top);
    case 3                                       % trapezoid
        xx_t = (yy_t-t2_top)./t1_top;
    case 4                                       % rectangular
        xx_t = W_half*ones(1,length(yy_t));
    case 5                                       % self-defined multiple planar facets
        k_B_t = zeros(1,n_seg_t);                % y_B = k_B*x_B + c_B
        c_B_t = zeros(1,n_seg_t);                % y_B = k_B*x_B + c_B
        xx_t = [];
        for i = 1:n_seg_t
            k_B_t(i) = (y_B_t(i+1)-y_B_t(i))/(x_B_t(i+1)-x_B_t(i));       % coefficients
            c_B_t(i) = y_B_t(i) - k_B_t(i)*x_B_t(i);                      % coefficients
            yy_t_tentative = ( ceil(V_roundn(y_B_t(i)/dy))-1:-1:ceil(V_roundn(y_B_t(i+1)/dy)) )*dy;
            xx_t_tentative = (yy_t_tentative-c_B_t(i))./k_B_t(i);
            xx_t = [xx_t xx_t_tentative];
        end
        xx_t(isnan(xx_t)) = x_B_t(end);
end

%%% Bottom part matrix ----------------------------------------------------
yy_b = -( 1:1:nh_bottom-1 )*dy;                  % increase yy_b, to calculate the length(residue) for x direction (Note, sign is considered)
%%% Pick the geometry shape for the bottom area
switch shape_bottom
    case 1                                       % circular or elliptic
        xx_b = R1_bottom*sqrt(1-(yy_b-Hs_bottom).^2/R2_bottom^2);
    case 2                                       % parabolic
        xx_b = sqrt((yy_b-p2_bottom)./p1_bottom);
    case 3                                       % trapezoid
        xx_b = (yy_b-t2_bottom)./t1_bottom;
    case 4                                       % rectangular
        xx_b = W_half*ones(1,length(yy_b));
    case 5                                       % self-defined multiple planar facets
        k_B_b = zeros(1,n_seg_b);                % y_B = k_B*x_B + c_B
        c_B_b = zeros(1,n_seg_b);                % y_B = k_B*x_B + c_B
        xx_b = [];
        i = 1;
        k_B_b(i) = (y_B_b(i+1)-y_B_b(i))/(x_B_b(i+1)-x_B_b(i));           % coefficients
        c_B_b(i) = y_B_b(i) - k_B_b(i)*x_B_b(i);                          % coefficients
        yy_b_tentative = -( 1:1:floor(V_roundn(abs(y_B_b(i+1))/dy)) )*dy;
        xx_b_tentative = (yy_b_tentative-c_B_b(i))./k_B_b(i);
        xx_b = [xx_b xx_b_tentative];
        for i = 2:n_seg_b
            k_B_b(i) = (y_B_b(i+1)-y_B_b(i))/(x_B_b(i+1)-x_B_b(i));       % coefficients
            c_B_b(i) = y_B_b(i) - k_B_b(i)*x_B_b(i);                      % coefficients
            yy_b_tentative = -( floor(V_roundn(abs(y_B_b(i))/dy))+1:1:floor(V_roundn(abs(y_B_b(i+1))/dy)) )*dy;
            xx_b_tentative = (yy_b_tentative-c_B_b(i))./k_B_b(i);
            xx_b = [xx_b xx_b_tentative];
        end
        xx_b(isnan(xx_b)) = x_B_b(1);
end

[Ix_t,Rx_t] = Func_Loc_Res_Coef(V_roundn(xx_t./dx));
[Ix_b,Rx_b] = Func_Loc_Res_Coef(V_roundn(xx_b./dx));

i = 1;
for p = nh_top-1:-1:0
    %%% atand(theta), theta is obtained in degree
    switch shape_top
        case 1                                                            % circular or elliptic
            theta = atand( (R1_top*(p*dy+Hs_top))/(R2_top*sqrt(R2_top^2-(p*dy+Hs_top)^2)) );
        case 2                                                            % parabolic
            theta = atand(-1/2/sqrt(p1_top*(p*dy-p2_top)));
        case 3                                                            % trapezoid
            if p == 0
               switch shape_bottom
                      case 1                                              % circular or elliptic
                            theta_b = atand( (R1_bottom*(p*dy-Hs_bottom))/(R2_bottom*sqrt(R2_bottom^2-(p*dy-Hs_bottom)^2)) );
                      case 2                                              % parabolic
                            theta_b = atand(-1/2/sqrt(p1_bottom*(p*dy-p2_bottom)));
                      case 3                                              % trapezoid
                            theta_b = atand(-1/t1_bottom);
                      case 4                                              % rectangular
                            theta_b = atand(0);
                      case 5                                              % self-defined multiple planar facets
                            k_B_b_tem = (y_B_b(2)-y_B_b(1))/(x_B_b(2)-x_B_b(1));
                            theta_b = atand(-1/k_B_b_tem);
               end
               theta = ( atand(-1/t1_top) + theta_b )/2;
            else
               theta = atand(-1/t1_top);
            end
        case 4                                                            % rectangular
            theta = atand(0);
        case 5                                                            % self-defined multiple planar facets
            i_index = find( y_B_t > p*dy, 1, 'last' );                    % find index of the segment
            if y_B_t(i_index+1) == p*dy && y_B_t(i_index+1)==0
               switch shape_bottom
                      case 1                                              % circular or elliptic
                            theta_b = atand( (R1_bottom*(p*dy-Hs_bottom))/(R2_bottom*sqrt(R2_bottom^2-(p*dy-Hs_bottom)^2)) );
                      case 2                                              % parabolic
                            theta_b = atand(-1/2/sqrt(p1_bottom*(p*dy-p2_bottom)));
                      case 3                                              % trapezoid
                            theta_b = atand(-1/t1_bottom);
                      case 4                                              % rectangular
                            theta_b = atand(0);
                      case 5                                              % self-defined multiple planar facets
                            k_B_b_tem = (y_B_b(2)-y_B_b(1))/(x_B_b(2)-x_B_b(1));
                            theta_b = atand(-1/k_B_b_tem);
               end
               theta = ( atand(-1/k_B_t(i_index)) + theta_b )/2;
            elseif y_B_t(i_index+1) == p*dy && y_B_t(i_index+1)~=0
               theta = ( atand(-1/k_B_t(i_index))+atand(-1/k_B_t(i_index+1)) )/2;
            else
               theta = atand(-1/k_B_t(i_index));
            end
    end
    
    ny = sind(theta);
    nx = cosd(theta);
    zr_1 = 1j/k0*(nx*nx/Z + Z*ny*ny);
    zr_2 = -1j/k0*(Z-1/Z)*nx*ny;
    zr_3 = zr_2;
    zr_4 = 1j/k0*(Z*nx*nx + ny*ny/Z);
    zl_1 = zr_1;
    zl_2 = -zr_2;
    zl_3 = zl_2;
    zl_4 = zr_4;
    Z_x1(i+1,x_c+Ix_t(i)) = zr_1;
    Z_x2(i+1,x_c+Ix_t(i)) = zr_2;
    Z_x3(i+1,x_c+Ix_t(i)) = zr_3;
    Z_x4(i+1,x_c+Ix_t(i)) = zr_4;
    Z_x1(i+1,x_c-Ix_t(i)) = zl_1;
    Z_x2(i+1,x_c-Ix_t(i)) = zl_2;
    Z_x3(i+1,x_c-Ix_t(i)) = zl_3;
    Z_x4(i+1,x_c-Ix_t(i)) = zl_4;
    a_x(i+1,x_c+Ix_t(i)-1) = Rx_t(i);

    R_x1(i+1,x_c+Ix_t(i)) = Rx_t(i);
    R_x3(i+1,x_c+Ix_t(i)) = Rx_t(i)/nx;
    R_x2(i+1,x_c+Ix_t(i)) = ny*Rx_t(i)/nx;
    R_x2_c(i+1,x_c+Ix_t(i)) = 1-ny*Rx_t(i)/nx;
    a_x(i+1,x_c-Ix_t(i)+1) = Rx_t(i);

    R_x1(i+1,x_c-Ix_t(i)) = Rx_t(i);
    R_x3(i+1,x_c-Ix_t(i)) = Rx_t(i)/nx;
    R_x2(i+1,x_c-Ix_t(i)) = ny*Rx_t(i)/nx;
    R_x2_c(i+1,x_c-Ix_t(i)) = 1-ny*Rx_t(i)/nx;
    a_x(i+1,x_c-Ix_t(i)+2:x_c+Ix_t(i)-2) = 1;

    R_in_d(i+1, x_c-Ix_t(i)+1:x_c+Ix_t(i)-1 ) = 1;
    i = i + 1;
end

i = 1;
for p = -1:-1:-(nh_bottom-1)
    switch shape_bottom
        case 1                                                            % circular or elliptic
            theta = atand( (R1_bottom*(p*dy-Hs_bottom))/(R2_bottom*sqrt(R2_bottom^2-(p*dy-Hs_bottom)^2)) );
        case 2                                                            % parabolic
            theta = atand(-1/2/sqrt(p1_bottom*(p*dy-p2_bottom)));
        case 3                                                            % trapezoid
            theta = atand(-1/t1_bottom);
        case 4                                                            % rectangular
            theta = atand(0);
        case 5                                                            % self-defined multiple planar facets
            i_index = find( y_B_b > p*dy, 1, 'last' );                    % find index of the segment
            if y_B_b(i_index+1) == p*dy
               theta = ( atand(-1/k_B_b(i_index))+atand(-1/k_B_b(i_index+1)) )/2;
            else
               theta = atand(-1/k_B_b(i_index));
            end
    end
    ny = sind(theta);
    nx = cosd(theta);
    zr_1 = 1j/k0*(nx*nx/Z + Z*ny*ny);
    zr_2 = -1j/k0*(Z-1/Z)*nx*ny;
    zr_3 = zr_2;
    zr_4 = 1j/k0*(Z*nx*nx + ny*ny/Z);
    zl_1 = zr_1;
    zl_2 = -zr_2;
    zl_3 = zl_2;
    zl_4 = zr_4;
    Z_x1(y_c+i,x_c+Ix_b(i)) = zr_1;
    Z_x2(y_c+i,x_c+Ix_b(i)) = zr_2;
    Z_x3(y_c+i,x_c+Ix_b(i)) = zr_3;
    Z_x4(y_c+i,x_c+Ix_b(i)) = zr_4;
    Z_x1(y_c+i,x_c-Ix_b(i)) = zl_1;
    Z_x2(y_c+i,x_c-Ix_b(i)) = zl_2;
    Z_x3(y_c+i,x_c-Ix_b(i)) = zl_3;
    Z_x4(y_c+i,x_c-Ix_b(i)) = zl_4;
    a_x(y_c+i,x_c+Ix_b(i)-1) = Rx_b(i);
    R_x1(y_c+i,x_c+Ix_b(i)) = Rx_b(i);
    R_x3(y_c+i,x_c+Ix_b(i)) = Rx_b(i)/abs(nx);
    R_x2(y_c+i,x_c+Ix_b(i)) = abs(ny)*Rx_b(i)/abs(nx);
    R_x2_c(y_c+i,x_c+Ix_b(i)) = 1-abs(ny)*Rx_b(i)/abs(nx);
    a_x(y_c+i,x_c-Ix_b(i)+1) = Rx_b(i);
    R_x1(y_c+i,x_c-Ix_b(i)) = Rx_b(i);
    R_x3(y_c+i,x_c-Ix_b(i)) = Rx_b(i)/abs(nx);
    R_x2(y_c+i,x_c-Ix_b(i)) = abs(ny)*Rx_b(i)/abs(nx);
    R_x2_c(y_c+i,x_c-Ix_b(i)) = 1-abs(ny)*Rx_b(i)/abs(nx);
    a_x(y_c+i,x_c-Ix_b(i)+2:x_c+Ix_b(i)-2) = 1;
    R_in_d(y_c+i, x_c-Ix_b(i)+1:x_c+Ix_b(i)-1 ) = 1;
    i = i + 1;
end

a_y = Inf(Ny+1,Nx+1);
R_y1 = zeros(Ny+1,Nx+1);
R_y2 = zeros(Ny+1,Nx+1);
R_y3 = zeros(Ny+1,Nx+1);
R_y1_c = zeros(Ny+1,Nx+1);
R_y1_n = zeros(Ny+1,Nx+1);
Z_y1 = zeros(Ny+1,Nx+1);
Z_y2 = zeros(Ny+1,Nx+1);
Z_y3 = zeros(Ny+1,Nx+1);
Z_y4 = zeros(Ny+1,Nx+1);

%%% Calculate the edge position for the top area
switch shape_top
    case 1                                       % circular or elliptic
        xxx_t_edge = X_t;
    case 2                                       % parabolic
        xxx_t_edge = X_t;
    case 3                                       % trapezoid
        xxx_t_edge = X_t;
    case 4                                       % rectangular
        xxx_t_edge = W_half;
    case 5                                       % self-defined multiple planar facets
        xxx_t_edge = 0;
end
[Ix_t_edge,~] = Func_Loc_Res_Coef(V_roundn(xxx_t_edge./dx));    % Ix_t_edge is number of points (including center point) horizontally from platform corner to center


%%% Top part matrix (from center axis to right) ---------------------------
xxx_t = (0:1:nw_half-1)*dx;                      % increase xxx_t, to calculate the length (residue) for y direction
%%% Pick the geometry shape for the top area
switch shape_top
    case 1                                       % circular or elliptic
        yyy_t = R2_top*sqrt(1-xxx_t.^2/R1_top^2) - Hs_top;
    case 2                                       % parabolic
        yyy_t = p1_top*xxx_t.^2 + p2_top;
    case 3                                       % trapezoid
        yyy_t = t1_top*xxx_t + t2_top;
    case 4                                       % rectangular
        yyy_t = H_top*ones(1,length(xxx_t));
    case 5                                       % self-defined multiple planar facets
        k_B_t = zeros(1,n_seg_t);                % y_B = k_B*x_B + c_B
        c_B_t = zeros(1,n_seg_t);                % y_B = k_B*x_B + c_B
        yyy_t = [];
        i = 1;
        k_B_t(i) = (y_B_t(i+1)-y_B_t(i))/(x_B_t(i+1)-x_B_t(i));           % coefficients
        c_B_t(i) = y_B_t(i) - k_B_t(i)*x_B_t(i);                          % coefficients
        xxx_t_tentative = ( 0:1:floor(V_roundn(x_B_t(i+1)/dx)) )*dx;
        yyy_t_tentative = k_B_t(i)*xxx_t_tentative + c_B_t(i);
        yyy_t = [yyy_t yyy_t_tentative];        
        for i = 2:n_seg_t
            k_B_t(i) = (y_B_t(i+1)-y_B_t(i))/(x_B_t(i+1)-x_B_t(i));       % coefficients
            c_B_t(i) = y_B_t(i) - k_B_t(i)*x_B_t(i);                      % coefficients
            xxx_t_tentative = ( floor(V_roundn(x_B_t(i)/dx))+1:1:floor(V_roundn(x_B_t(i+1)/dx)) )*dx;
            yyy_t_tentative = k_B_t(i)*xxx_t_tentative + c_B_t(i);
            if isempty(yyy_t_tentative)
                yyy_t_tentative = 0;
            end
            yyy_t = [yyy_t yyy_t_tentative];
        end
        yyy_t = yyy_t(1:end-1);
end
yyy_t(1:Ix_t_edge) = H_top;                      % if no platform, at the top point, Ix_t_edge=0

%%% Bottom part matrix (from center axis to right) ------------------------
xxx_b = (0:1:nw_half-1)*dx;                      % increase xxx_b, to calculate the length (residue) for y direction
%%% Calculate the edge position for the bottom area
switch shape_bottom
    case 1                                       % circular or elliptic
        xxx_b_edge = X_b;
    case 2                                       % parabolic
        xxx_b_edge = X_b;
    case 3                                       % trapezoid
        xxx_b_edge = X_b;
    case 4                                       % rectangular
        xxx_b_edge = W_half;
    case 5                                       % self-defined multiple planar facets
        xxx_b_edge = 0;
end
[Ix_b_edge,~] = Func_Loc_Res_Coef(V_roundn(xxx_b_edge./dx));

%%% Pick the geometry shape for the bottom area
switch shape_bottom
    case 1                                       % circular or elliptic
        yyy_b = -R2_bottom*sqrt(1-xxx_b.^2/R1_bottom^2) + Hs_bottom;
    case 2                                       % parabolic
        yyy_b = p1_bottom*xxx_b.^2 + p2_bottom;
    case 3                                       % trapezoid
        yyy_b = t1_bottom*xxx_b + t2_bottom;
    case 4                                       % rectangular
        yyy_b = -H_bottom*ones(1,length(xxx_b));
    case 5                                       % self-defined multiple planar facets
        k_B_b = zeros(1,n_seg_b);                % y_B = k_B*x_B + c_B
        c_B_b = zeros(1,n_seg_b);                % y_B = k_B*x_B + c_B
        yyy_b = [];
        i = n_seg_b;
        k_B_b(i) = (y_B_b(i+1)-y_B_b(i))/(x_B_b(i+1)-x_B_b(i));           % coefficients
        c_B_b(i) = y_B_b(i) - k_B_b(i)*x_B_b(i);                          % coefficients
        xxx_b_tentative = ( 0:1:floor(V_roundn(x_B_b(i)/dx)) )*dx;
        yyy_b_tentative = k_B_b(i)*xxx_b_tentative + c_B_b(i);
        yyy_b = [yyy_b yyy_b_tentative];        
        for i = n_seg_b-1:-1:1
            k_B_b(i) = (y_B_b(i+1)-y_B_b(i))/(x_B_b(i+1)-x_B_b(i));       % coefficients
            c_B_b(i) = y_B_b(i) - k_B_b(i)*x_B_b(i);                      % coefficients
            xxx_b_tentative = ( floor(V_roundn(x_B_b(i+1)/dx))+1:1:floor(V_roundn(x_B_b(i)/dx)) )*dx;
            yyy_b_tentative = k_B_b(i)*xxx_b_tentative + c_B_b(i);
            if isempty(yyy_b_tentative)
                yyy_b_tentative = 0;
            end
            yyy_b = [yyy_b yyy_b_tentative];  
        end
        yyy_b = yyy_b(1:end-1);
end
yyy_b(1:Ix_b_edge) = -H_bottom;                  % if no platform, at the bottom point, Ix_b_edge=0

[Iy_t,Ry_t] = Func_Loc_Res_Coef(V_roundn(yyy_t./dy));
[Iy_b,Ry_b] = Func_Loc_Res_Coef(V_roundn(yyy_b./dy));

%%% Platform counterpart impedance and boundary coefficients --------------
j = 0;
for q = 0:1:Ix_b_edge-1
    if j == 49
        j = j;
    end
    switch shape_top
        case 1                                   % circular or elliptic
            theta = atand( R1_top*sqrt(R1_top^2-(q*dx)^2)/(R2_top*q*dx) );
        case 2                                   % parabolic
            theta = atand( -1/2/p1_top/(q*dx) );
        case 3                                   % trapezoid
            if q*dx < X_b
                theta = atand(Inf);
            elseif q*dx == X_b
                theta = (atand(Inf) + atand(-1/t1_top) )/2;
            else
                theta = atand(-1/t1_top);
            end
       case 4                                    % rectangular
            theta = atand(Inf);
    end
    ny = sind(theta);
    nx = cosd(theta);
    zr_1 = 1j/k0*(nx*nx/Z + Z*ny*ny);
    zr_2 = -1j/k0*(Z-1/Z)*nx*ny;
    zr_3 = zr_2;
    zr_4 = 1j/k0*(Z*nx*nx + ny*ny/Z);
    zl_1 = zr_1;
    zl_2 = -zr_2;
    zl_3 = zl_2;
    zl_4 = zr_4;
    Z_y1(y_c-Iy_t(j+1),x_c+j) = zr_1;
    Z_y2(y_c-Iy_t(j+1),x_c+j) = zr_2;
    Z_y3(y_c-Iy_t(j+1),x_c+j) = zr_3;
    Z_y4(y_c-Iy_t(j+1),x_c+j) = zr_4;
    Z_y1(y_c-Iy_t(j+1),x_c-j) = zl_1;
    Z_y2(y_c-Iy_t(j+1),x_c-j) = zl_2;
    Z_y3(y_c-Iy_t(j+1),x_c-j) = zl_3;
    Z_y4(y_c-Iy_t(j+1),x_c-j) = zl_4;
    a_y(y_c-Iy_t(j+1)+1,x_c+j) = Ry_t(j+1);
    R_y2(y_c-Iy_t(j+1),x_c+j) = Ry_t(j+1);
    R_y3(y_c-Iy_t(j+1),x_c+j) = Ry_t(j+1)/ny;
    R_y1(y_c-Iy_t(j+1),x_c+j) = nx*Ry_t(j+1)/ny;
    R_y1_c(y_c-Iy_t(j+1),x_c+j) = 1-nx*Ry_t(j+1)/ny;
    a_y(y_c-Iy_t(j+1)+1,x_c-j) = Ry_t(j+1);
    R_y2(y_c-Iy_t(j+1),x_c-j) = Ry_t(j+1);
    R_y3(y_c-Iy_t(j+1),x_c-j) = Ry_t(j+1)/ny;
    R_y1(y_c-Iy_t(j+1),x_c-j) = nx*Ry_t(j+1)/ny;
    R_y1_c(y_c-Iy_t(j+1),x_c-j) = 1-nx*Ry_t(j+1)/ny;
    theta = atand(-Inf);
    ny = sind(theta);
    nx = cosd(theta);
    zr_1 = 1j/k0*(nx*nx/Z + Z*ny*ny);
    zr_2 = -1j/k0*(Z-1/Z)*nx*ny;
    zr_3 = zr_2;
    zr_4 = 1j/k0*(Z*nx*nx + ny*ny/Z);
    zl_1 = zr_1;
    zl_2 = -zr_2;
    zl_3 = zl_2;
    zl_4 = zr_4;
    Z_y1(y_c+Iy_b(j+1),x_c+j) = zr_1;
    Z_y2(y_c+Iy_b(j+1),x_c+j) = zr_2;
    Z_y3(y_c+Iy_b(j+1),x_c+j) = zr_3;
    Z_y4(y_c+Iy_b(j+1),x_c+j) = zr_4;
    Z_y1(y_c+Iy_b(j+1),x_c-j) = zl_1;
    Z_y2(y_c+Iy_b(j+1),x_c-j) = zl_2;
    Z_y3(y_c+Iy_b(j+1),x_c-j) = zl_3;
    Z_y4(y_c+Iy_b(j+1),x_c-j) = zl_4;
    a_y(y_c+Iy_b(j+1)-1,x_c+j) = Ry_b(j+1);
    R_y2(y_c+Iy_b(j+1),x_c+j) = Ry_b(j+1);
    R_y3(y_c+Iy_b(j+1),x_c+j) = Ry_b(j+1)/abs(ny);
    R_y1(y_c+Iy_b(j+1),x_c+j) = abs(nx)*Ry_b(j+1)/abs(ny);
    R_y1_c(y_c+Iy_b(j+1),x_c+j) = 1-abs(nx)*Ry_b(j+1)/abs(ny);
    a_y(y_c+Iy_b(j+1)-1,x_c-j) = Ry_b(j+1);
    R_y2(y_c+Iy_b(j+1),x_c-j) = Ry_b(j+1);
    R_y3(y_c+Iy_b(j+1),x_c-j) = Ry_b(j+1)/abs(ny);
    R_y1(y_c+Iy_b(j+1),x_c-j) = abs(nx)*Ry_b(j+1)/abs(ny);
    R_y1_c(y_c+Iy_b(j+1),x_c-j) = 1-abs(nx)*Ry_b(j+1)/abs(ny);
    a_y(y_c-Iy_t(j+1)+2:y_c+Iy_b(j+1)-2,x_c-j) = 1;
    a_y(y_c-Iy_t(j+1)+2:y_c+Iy_b(j+1)-2,x_c+j) = 1;
    j = j + 1;
end

j = Ix_b_edge;
for q = Ix_b_edge:1:nw_half-1
    switch shape_top
        case 1                                   % circular or elliptic
            theta = atand( R1_top*sqrt(R1_top^2-(q*dx)^2)/(R2_top*q*dx) );
        case 2                                   % parabolic
            theta = atand( -1/2/p1_top/(q*dx) );
        case 3                                   % trapezoid
            theta = atand(-1/t1_top);
        case 4                                   % rectangular
            theta = atand(Inf);
        case 5                                   % self-defined multiple planar facets
            i_index = find( x_B_t >= q*dx, 1, 'first' ); % find index of the segment
            if x_B_t(i_index) == q*dx && q*dx == 0
               theta = atand(Inf); 
            elseif x_B_t(i_index) == q*dx && q*dx ~= 0
%              theta = ( atand(-1/k_B_t(i_index-1))+atand(-1/k_B_t(i_index)) )/2;
               theta = ( 90 + atand(-1/k_B_t(i_index)) )/2;
            else
               theta = atand(-1/k_B_t(i_index-1));
               if theta == -90
                  theta = 90;
               end
            end
    end
    ny = sind(theta);
    nx = cosd(theta);
    zr_1 = 1j/k0*(nx*nx/Z + Z*ny*ny);
    zr_2 = -1j/k0*(Z-1/Z)*nx*ny;
    zr_3 = zr_2;
    zr_4 = 1j/k0*(Z*nx*nx + ny*ny/Z);
    zl_1 = zr_1;
    zl_2 = -zr_2;
    zl_3 = zl_2;
    zl_4 = zr_4;
    Z_y1(y_c-Iy_t(j+1),x_c+j) = zr_1;
    Z_y2(y_c-Iy_t(j+1),x_c+j) = zr_2;
    Z_y3(y_c-Iy_t(j+1),x_c+j) = zr_3;
    Z_y4(y_c-Iy_t(j+1),x_c+j) = zr_4;
    Z_y1(y_c-Iy_t(j+1),x_c-j) = zl_1;
    Z_y2(y_c-Iy_t(j+1),x_c-j) = zl_2;
    Z_y3(y_c-Iy_t(j+1),x_c-j) = zl_3;
    Z_y4(y_c-Iy_t(j+1),x_c-j) = zl_4;
    a_y(y_c-Iy_t(j+1)+1,x_c+j) = Ry_t(j+1);
    R_y2(y_c-Iy_t(j+1),x_c+j) = Ry_t(j+1);
    R_y3(y_c-Iy_t(j+1),x_c+j) = Ry_t(j+1)/ny;
    R_y1(y_c-Iy_t(j+1),x_c+j) = nx*Ry_t(j+1)/ny;
    R_y1_c(y_c-Iy_t(j+1),x_c+j) = 1-nx*Ry_t(j+1)/ny;
    a_y(y_c-Iy_t(j+1)+1,x_c-j) = Ry_t(j+1);
    R_y2(y_c-Iy_t(j+1),x_c-j) = Ry_t(j+1);
    R_y3(y_c-Iy_t(j+1),x_c-j) = Ry_t(j+1)/ny;
    R_y1(y_c-Iy_t(j+1),x_c-j) = nx*Ry_t(j+1)/ny;
    R_y1_c(y_c-Iy_t(j+1),x_c-j) = 1-nx*Ry_t(j+1)/ny;

    switch shape_bottom
        case 1                                   % circular or elliptic
            theta = atand( -R1_bottom*sqrt(R1_bottom^2-(q*dx)^2)/(R2_bottom*q*dx) );
        case 2                                   % parabolic
            theta = atand( -1/2/p1_bottom/(q*dx) );
        case 3                                   % trapezoid
            theta = atand(-1/t1_bottom);
        case 4                                   % rectangular
            theta = atand(-Inf);
        case 5                                   % self-defined multiple planar facets
            x_B_b_tem = x_B_b(end:-1:1);
            k_B_b_tem = k_B_b(end:-1:1);
            i_index = find( x_B_b_tem >= q*dx, 1, 'first' ); % find index of the segment
            if x_B_b_tem(i_index) == q*dx && q*dx == 0
               theta = atand(-Inf); 
            elseif x_B_b_tem(i_index) == q*dx && q*dx ~= 0
%              theta = ( atand(-1/k_B_b_tem(i_index-1))+atand(-1/k_B_b_tem(i_index)) )/2;
               theta = ( -90 + atand(-1/k_B_b_tem(i_index)) )/2;
            else
               theta = atand(-1/k_B_b_tem(i_index-1));
               if theta == 90
                  theta = -90;
               end
            end
    end
    ny = sind(theta);
    nx = cosd(theta);
    zr_1 = 1j/k0*(nx*nx/Z + Z*ny*ny);
    zr_2 = -1j/k0*(Z-1/Z)*nx*ny;
    zr_3 = zr_2;
    zr_4 = 1j/k0*(Z*nx*nx + ny*ny/Z);
    zl_1 = zr_1;
    zl_2 = -zr_2;
    zl_3 = zl_2;
    zl_4 = zr_4;
    Z_y1(y_c+Iy_b(j+1),x_c+j) = zr_1;
    Z_y2(y_c+Iy_b(j+1),x_c+j) = zr_2;
    Z_y3(y_c+Iy_b(j+1),x_c+j) = zr_3;
    Z_y4(y_c+Iy_b(j+1),x_c+j) = zr_4;
    Z_y1(y_c+Iy_b(j+1),x_c-j) = zl_1;
    Z_y2(y_c+Iy_b(j+1),x_c-j) = zl_2;
    Z_y3(y_c+Iy_b(j+1),x_c-j) = zl_3;
    Z_y4(y_c+Iy_b(j+1),x_c-j) = zl_4;
    a_y(y_c+Iy_b(j+1)-1,x_c+j) = Ry_b(j+1);
    R_y2(y_c+Iy_b(j+1),x_c+j) = Ry_b(j+1);
    R_y3(y_c+Iy_b(j+1),x_c+j) = Ry_b(j+1)/abs(ny);
    R_y1(y_c+Iy_b(j+1),x_c+j) = abs(nx)*Ry_b(j+1)/abs(ny);
    R_y1_c(y_c+Iy_b(j+1),x_c+j) = 1-abs(nx)*Ry_b(j+1)/abs(ny);
    a_y(y_c+Iy_b(j+1)-1,x_c-j) = Ry_b(j+1);
    R_y2(y_c+Iy_b(j+1),x_c-j) = Ry_b(j+1);
    R_y3(y_c+Iy_b(j+1),x_c-j) = Ry_b(j+1)/abs(ny);
    R_y1(y_c+Iy_b(j+1),x_c-j) = abs(nx)*Ry_b(j+1)/abs(ny);
    R_y1_c(y_c+Iy_b(j+1),x_c-j) = 1-abs(nx)*Ry_b(j+1)/abs(ny);
    a_y(y_c-Iy_t(j+1)+2:y_c+Iy_b(j+1)-2,x_c-j) = 1;
    a_y(y_c-Iy_t(j+1)+2:y_c+Iy_b(j+1)-2,x_c+j) = 1;
    j = j + 1;
end

[rx_x,rx_y] = find(R_x2 >1);
for i = 1:length(rx_x)
    R_x2_c(rx_x(i),rx_y(i)) = 0;
    R_x2_n(rx_x(i),rx_y(i)) = R_x1(rx_x(i),rx_y(i))*(R_x2(rx_x(i),rx_y(i))-1)/R_x2(rx_x(i),rx_y(i));
    R_x3(rx_x(i),rx_y(i)) = R_x3(rx_x(i),rx_y(i))*( 1 - (R_x2(rx_x(i),rx_y(i))-1)/R_x2(rx_x(i),rx_y(i)) );
    R_x2(rx_x(i),rx_y(i)) = 1 - R_x2_n(rx_x(i),rx_y(i));
end
[ry_x,ry_y] = find(R_y1 >1);
for i = 1:length(ry_x)
    R_y1_c(ry_x(i),ry_y(i)) = 0;
    R_y1_n(ry_x(i),ry_y(i)) = R_y2(ry_x(i),ry_y(i))*(R_y1(ry_x(i),ry_y(i))-1)/R_y1(ry_x(i),ry_y(i));
    R_y3(ry_x(i),ry_y(i)) = R_y3(ry_x(i),ry_y(i))*( 1 - (R_y1(ry_x(i),ry_y(i))-1)/R_y1(ry_x(i),ry_y(i)) );
    R_y1(ry_x(i),ry_y(i)) = 1 - R_y1_n(ry_x(i),ry_y(i));
end
R_B_d1 = ceil(R_x1);
R_B_d2 = ceil(R_y2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Coefficients
rx = ds/dx/dx;
ry = ds/dy/dy;
cx = rx/(4*1j*k0);
cy = ry/(4*1j*k0);
bc_t_A1 = zeros(1,2*Nx+1);
bc_b_A1 = zeros(1,2*Nx+1);
d_t_xy_A1 = zeros(1,Nx);
d_b_xy_A1 = zeros(1,Nx+2);
d_t_yx_A1 = zeros(1,Nx+2);
d_b_yx_A1 = zeros(1,Nx);
bc_t_A1_u = zeros(1,2*Nx+1);
bc_b_A1_u = zeros(1,2*Nx+1);
d_t_xy_A1_u = zeros(1,Nx);
d_b_xy_A1_u = zeros(1,Nx+2);
d_t_yx_A1_u = zeros(1,Nx+2);
d_b_yx_A1_u = zeros(1,Nx);
bc_t_A1_d = zeros(1,2*Nx+1);
bc_b_A1_d = zeros(1,2*Nx+1);
d_t_xy_A1_d = zeros(1,Nx);
d_b_xy_A1_d = zeros(1,Nx+2);
d_t_yx_A1_d = zeros(1,Nx+2);
d_b_yx_A1_d = zeros(1,Nx);
bc_t_A2 = zeros(2*Ny+1,1);
bc_b_A2 = zeros(2*Ny+1,1);
d_t_xy_A2 = zeros(Ny,1);
d_b_xy_A2 = zeros(Ny+2,1);
d_t_yx_A2 = zeros(Ny+2,1);
d_b_yx_A2 = zeros(Ny,1);
bc_t_A2_l = zeros(2*Ny+1,1);
bc_b_A2_l = zeros(2*Ny+1,1);
d_t_xy_A2_l = zeros(Ny,1);
d_b_xy_A2_l = zeros(Ny+2,1);
d_t_yx_A2_l = zeros(Ny+2,1);
d_b_yx_A2_l = zeros(Ny,1);
bc_t_A2_r = zeros(2*Ny+1,1);
bc_b_A2_r = zeros(2*Ny+1,1);
d_t_xy_A2_r = zeros(Ny,1);
d_b_xy_A2_r = zeros(Ny+2,1);
d_t_yx_A2_r = zeros(Ny+2,1);
d_b_yx_A2_r = zeros(Ny,1);
bx_1 = zeros(Nx+1,1);
bx_1_u = zeros(Nx+1,1);
bx_1_d = zeros(Nx+1,1);
by_1 = bx_1;
by_1_u = bx_1_u;
by_1_d = bx_1_d;
bx_2 = zeros(Ny+1,1);
bx_2_l = zeros(Ny+1,1);
bx_2_r = zeros(Ny+1,1);
by_2 = bx_2;
by_2_l = bx_2_l;
by_2_r = bx_2_r;

C_d_x = (dx.*R_x3 - Z_x1).*(dx.*R_x3 - Z_x4) - Z_x2.*Z_x3;
C1_x = -( Z_x1.*(dx.*R_x3 - Z_x4) + Z_x2.*Z_x3 )./C_d_x;
C3_x = -( Z_x4.*(dx.*R_x3 - Z_x1) + Z_x2.*Z_x3 )./C_d_x;
C2_x = -( Z_x3.*(dx.*R_x3 - Z_x4) + Z_x3.*Z_x4 )./C_d_x;
C4_x = -( Z_x2.*(dx.*R_x3 - Z_x1) + Z_x1.*Z_x2 )./C_d_x;
C_d_y = (dy.*R_y3 - Z_y1).*(dy.*R_y3 - Z_y4) - Z_y2.*Z_y3;
C1_y = -( Z_y1.*(dy.*R_y3 - Z_y4) + Z_y2.*Z_y3 )./C_d_y;
C3_y = -( Z_y4.*(dy.*R_y3 - Z_y1) + Z_y2.*Z_y3 )./C_d_y;
C2_y = -( Z_y3.*(dy.*R_y3 - Z_y4) + Z_y3.*Z_y4 )./C_d_y;
C4_y = -( Z_y2.*(dy.*R_y3 - Z_y1) + Z_y1.*Z_y2 )./C_d_y;
C1_x = C1_x.*R_B_d1;
C2_x = C2_x.*R_B_d1;
C3_x = C3_x.*R_B_d1;
C4_x = C4_x.*R_B_d1;
C1_y = C1_y.*R_B_d2;
C2_y = C2_y.*R_B_d2;
C3_y = C3_y.*R_B_d2;
C4_y = C4_y.*R_B_d2;
C1_x(isnan(C1_x)) = 0;
C2_x(isnan(C2_x)) = 0;
C3_x(isnan(C3_x)) = 0;
C4_x(isnan(C4_x)) = 0;
C1_y(isnan(C1_y)) = 0;
C2_y(isnan(C2_y)) = 0;
C3_y(isnan(C3_y)) = 0;
C4_y(isnan(C4_y)) = 0;

%%%------------------------------------------------------------------------
%%% Adjust the matrix if BC is PEC or PMC
%%% Note when BC is PEC or PMC, only one of Ex and Ey is required
if BC == 2                      % PEC BC
   C1_x = 0*R_B_d1;
   C2_x = 0*R_B_d1;             % there is no coupling between Ex and Ey, only one of them is needed
   C3_x = 0*R_B_d1;
   C4_x = 0*R_B_d1;             % there is no coupling between Ex and Ey, only one of them is needed
   C1_y = 0*R_B_d2;
   C2_y = 0*R_B_d2;             % there is no coupling between Ex and Ey, only one of them is needed
   C3_y = 0*R_B_d2;
   C4_y = 0*R_B_d2;             % there is no coupling between Ex and Ey, only one of them is needed
elseif BC == 3                  % PMC BC
   C1_x = R_B_d1;
   C2_x = 0*R_B_d1;         % there is no coupling between Ex and Ey, only one of them is needed
   C3_x = R_B_d1;
   C4_x = 0*R_B_d1;         % there is no coupling between Ex and Ey, only one of them is needed
   C1_y = R_B_d2;
   C2_y = 0*R_B_d2;         % there is no coupling between Ex and Ey, only one of them is needed
   C3_y = R_B_d2;
   C4_y = 0*R_B_d2;         % there is no coupling between Ex and Ey, only one of them is needed
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Initial plane
Tx_vertical = TX{2} - H_bottom;
Tx_horizontal = TX{1};
w0 = k_w0*lambda;
u_g_c = zeros(Ny+1,Nx+1);
C = 1;
n_GB_x = 1;
n_GB_y = n_GB_x;
n_GB = n_GB_x*n_GB_y;
w0_x = zeros(1,n_GB);
w0_y = zeros(1,n_GB);
z_far = 0;
for ii = 1:n_GB_x
    w0_x((ii-1)*n_GB_x+1:ii*n_GB_x) = w0(ii);
    w0_y((ii-1)*n_GB_y+1:ii*n_GB_y) = w0;
end
w_x = w0_x.*sqrt(1 + (lambda*z_far/pi)^2./w0_x.^4);
R_x = z_far + pi^2./lambda^2./z_far.*w0_x.^4;
phi0_x = atan(lambda.*z_far./pi./w0_x.^2);
w_y = w0_y.*sqrt(1 + (lambda*z_far/pi)^2./w0_y.^4);
R_y = z_far + pi^2./lambda^2./z_far.*w0_y.^4;
phi0_y = atan(lambda.*z_far./pi./w0_y.^2);
iii=1;
for ny = nh_top:-1:-nh_bottom
    jjj=1;
    for nx = -nw_half:1:nw_half
        u_g_c(iii,jjj) = sqrt(2./pi./(w_x.*w_y)).*exp(-((nx*dx-Tx_horizontal)^2./w_x.^2 + (ny*dy-Tx_vertical)^2./w_y.^2) - 1j*k0*z_far...
                         - 1j.*pi.*((nx*dx-Tx_horizontal)^2./lambda./R_x + (ny*dy-Tx_vertical)^2./lambda./R_y) + 1j.*phi0_x/2 + 1j.*phi0_y/2)*C;
          jjj = jjj+1;
    end
    iii = iii+1;    
end
Source_x = 0;
Source_y = u_g_c/max(max(u_g_c));
Ex = Source_x.*R_in_d;
Ey = Source_y.*R_in_d;

y_B_c = find(R_B_d2(:,x_c)==1);
Ex(y_B_c(1),x_c) = C1_y(y_B_c(1),x_c)*Ex(y_B_c(1)+1,x_c) + C2_y(y_B_c(1),x_c)*Ey(y_B_c(1)+1,x_c);
Ey(y_B_c(1),x_c) = C3_y(y_B_c(1),x_c)*Ey(y_B_c(1)+1,x_c) + C4_y(y_B_c(1),x_c)*Ex(y_B_c(1)+1,x_c);
Ex(y_B_c(2),x_c) = C1_y(y_B_c(2),x_c)*Ex(y_B_c(2)-1,x_c) + C2_y(y_B_c(2),x_c)*Ey(y_B_c(2)-1,x_c);
Ey(y_B_c(2),x_c) = C3_y(y_B_c(2),x_c)*Ey(y_B_c(2)-1,x_c) + C4_y(y_B_c(2),x_c)*Ex(y_B_c(2)-1,x_c);
for jj = 1:nw_half-1

    x_r = x_c+jj ;
    x_l = x_c-jj ;
    y_B_r = find(R_B_d2(:,x_r)==1);
    y_B_l = find(R_B_d2(:,x_l)==1);
    Ex(y_B_r(1),x_r) = C1_y(y_B_r(1),x_r)*( R_y1(y_B_r(1),x_r)*Ex(y_B_r(1)+1,x_r-1) + R_y1_c(y_B_r(1),x_r)*Ex(y_B_r(1)+1,x_r) + R_y1_n(y_B_r(1),x_r)*Ex(y_B_r(1),x_r-1) )...
                       + C2_y(y_B_r(1),x_r)*( R_y1(y_B_r(1),x_r)*Ey(y_B_r(1)+1,x_r-1) + R_y1_c(y_B_r(1),x_r)*Ey(y_B_r(1)+1,x_r) + R_y1_n(y_B_r(1),x_r)*Ey(y_B_r(1),x_r-1) );
    Ey(y_B_r(1),x_r) = C3_y(y_B_r(1),x_r)*( R_y1(y_B_r(1),x_r)*Ey(y_B_r(1)+1,x_r-1) + R_y1_c(y_B_r(1),x_r)*Ey(y_B_r(1)+1,x_r) + R_y1_n(y_B_r(1),x_r)*Ey(y_B_r(1),x_r-1) )...
                       + C4_y(y_B_r(1),x_r)*( R_y1(y_B_r(1),x_r)*Ex(y_B_r(1)+1,x_r-1) + R_y1_c(y_B_r(1),x_r)*Ex(y_B_r(1)+1,x_r) + R_y1_n(y_B_r(1),x_r)*Ex(y_B_r(1),x_r-1) );
    Ex(y_B_r(2),x_r) = C1_y(y_B_r(2),x_r)*( R_y1(y_B_r(2),x_r)*Ex(y_B_r(2)-1,x_r-1) + R_y1_c(y_B_r(2),x_r)*Ex(y_B_r(2)-1,x_r) + R_y1_n(y_B_r(2),x_r)*Ex(y_B_r(2),x_r-1) )...
                       + C2_y(y_B_r(2),x_r)*( R_y1(y_B_r(2),x_r)*Ey(y_B_r(2)-1,x_r-1) + R_y1_c(y_B_r(2),x_r)*Ey(y_B_r(2)-1,x_r) + R_y1_n(y_B_r(2),x_r)*Ey(y_B_r(2),x_r-1) );
    Ey(y_B_r(2),x_r) = C3_y(y_B_r(2),x_r)*( R_y1(y_B_r(2),x_r)*Ey(y_B_r(2)-1,x_r-1) + R_y1_c(y_B_r(2),x_r)*Ey(y_B_r(2)-1,x_r) + R_y1_n(y_B_r(2),x_r)*Ey(y_B_r(2),x_r-1) )...
                       + C4_y(y_B_r(2),x_r)*( R_y1(y_B_r(2),x_r)*Ex(y_B_r(2)-1,x_r-1) + R_y1_c(y_B_r(2),x_r)*Ex(y_B_r(2)-1,x_r) + R_y1_n(y_B_r(2),x_r)*Ex(y_B_r(2),x_r-1) );
    Ex(y_B_l(1),x_l) = C1_y(y_B_l(1),x_l)*( R_y1(y_B_l(1),x_l)*Ex(y_B_l(1)+1,x_l+1) + R_y1_c(y_B_l(1),x_l)*Ex(y_B_l(1)+1,x_l) + R_y1_n(y_B_l(1),x_l)*Ex(y_B_l(1),x_l+1) )...
                       + C2_y(y_B_l(1),x_l)*( R_y1(y_B_l(1),x_l)*Ey(y_B_l(1)+1,x_l+1) + R_y1_c(y_B_l(1),x_l)*Ey(y_B_l(1)+1,x_l) + R_y1_n(y_B_l(1),x_l)*Ey(y_B_l(1),x_l+1) );
    Ey(y_B_l(1),x_l) = C3_y(y_B_l(1),x_l)*( R_y1(y_B_l(1),x_l)*Ey(y_B_l(1)+1,x_l+1) + R_y1_c(y_B_l(1),x_l)*Ey(y_B_l(1)+1,x_l) + R_y1_n(y_B_l(1),x_l)*Ey(y_B_l(1),x_l+1) )...
                       + C4_y(y_B_l(1),x_l)*( R_y1(y_B_l(1),x_l)*Ex(y_B_l(1)+1,x_l+1) + R_y1_c(y_B_l(1),x_l)*Ex(y_B_l(1)+1,x_l) + R_y1_n(y_B_l(1),x_l)*Ex(y_B_l(1),x_l+1) );
    Ex(y_B_l(2),x_l) = C1_y(y_B_l(2),x_l)*( R_y1(y_B_l(2),x_l)*Ex(y_B_l(2)-1,x_l+1) + R_y1_c(y_B_l(2),x_l)*Ex(y_B_l(2)-1,x_l) + R_y1_n(y_B_l(2),x_l)*Ex(y_B_l(2),x_l+1) )...
                       + C2_y(y_B_l(2),x_l)*( R_y1(y_B_l(2),x_l)*Ey(y_B_l(2)-1,x_l+1) + R_y1_c(y_B_l(2),x_l)*Ey(y_B_l(2)-1,x_l) + R_y1_n(y_B_l(2),x_l)*Ey(y_B_l(2),x_l+1) );
    Ey(y_B_l(2),x_l) = C3_y(y_B_l(2),x_l)*( R_y1(y_B_l(2),x_l)*Ey(y_B_l(2)-1,x_l+1) + R_y1_c(y_B_l(2),x_l)*Ey(y_B_l(2)-1,x_l) + R_y1_n(y_B_l(2),x_l)*Ey(y_B_l(2),x_l+1) )...
                       + C4_y(y_B_l(2),x_l)*( R_y1(y_B_l(2),x_l)*Ex(y_B_l(2)-1,x_l+1) + R_y1_c(y_B_l(2),x_l)*Ex(y_B_l(2)-1,x_l) + R_y1_n(y_B_l(2),x_l)*Ex(y_B_l(2),x_l+1) );

end
Wx_m = zeros(Ny+1,Nx+1);
Wy_m = zeros(Ny+1,Nx+1);
Ex_all = zeros(Ny+1,Nx+1,Nz+1);
Ey_all = zeros(Ny+1,Nx+1,Nz+1);
Ex_all(:,:,1) = Ex;
Ey_all(:,:,1) = Ey;
Wx = Ex*exp(1j*k0*s_start);
Wy = Ey*exp(1j*k0*s_start);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for z = 1:Nz
    d_c_A1 = 1 + (2*cx)./a_x(y_c,:);
    d_t_A1 = (-2*cx)./(1+a_x(y_c,1:Nx))./([ones(1,x_c) a_x(y_c,x_c+1:Nx)]);
    d_b_A1 = (-2*cx)./(1+a_x(y_c,2:Nx+1))./([a_x(y_c,2:nw_half) ones(1,x_c)]);
    bc_t_A1(1:nw_half) = C1_x(y_c,1:nw_half).*R_x2_c(y_c,1:nw_half);
    bc_t_A1(Nx+2:Nx+x_c) = C3_x(y_c,1:nw_half).*R_x2_c(y_c,1:nw_half);
    bc_b_A1(x_c:Nx) = C1_x(y_c,x_c+1:Nx+1).*R_x2_c(y_c,x_c+1:Nx+1);
    bc_b_A1(Nx+x_c+1:2*Nx+1) = C3_x(y_c,x_c+1:Nx+1).*R_x2_c(y_c,x_c+1:Nx+1);
    d_t_xy_A1(1:nw_half) = -C2_x(y_c,1:nw_half).*R_x2_c(y_c,1:nw_half);
    d_b_xy_A1(x_c+1:Nx+1) = -C2_x(y_c,x_c+1:Nx+1).*R_x2_c(y_c,x_c+1:Nx+1);
    d_t_yx_A1(2:x_c) = -C4_x(y_c,1:nw_half).*R_x2_c(y_c,1:nw_half);
    d_b_yx_A1(x_c:Nx) = -C4_x(y_c,x_c+1:Nx+1).*R_x2_c(y_c,x_c+1:Nx+1);
    A1_c = diag([d_c_A1 d_c_A1], 0) + diag(([d_t_A1 0 d_t_A1] - bc_t_A1), 1) + diag(([d_b_A1 0 d_b_A1] - bc_b_A1), -1)...
           + diag(d_t_xy_A1, Nx+2) + diag(d_b_xy_A1, Nx) + diag(d_b_yx_A1, -Nx-2) + diag(d_t_yx_A1, -Nx);
        
    for j = 2:Nx
        bx_1(j) = cy*Wx(y_c-1,j) + (1-2*cy)*Wx(y_c,j) + cy*Wx(y_c+1,j);
        by_1(j) = cy*Wy(y_c-1,j) + (1-2*cy)*Wy(y_c,j) + cy*Wy(y_c+1,j);
    end
    
    w1_c = A1_c\( [ bx_1.*(R_in_d(y_c,:).') ; by_1.*(R_in_d(y_c,:).') ] );
    Wx_m(y_c,:) = w1_c(1:Nx+1).';
    Wy_m(y_c,:) = w1_c(Nx+2:end).';
    
    for i = 1:min(nh_bottom, nh_top) -1
        y_u = y_c-i ;
        y_d = y_c+i ;
        d_c_A1_u = 1 + (2*cx)./a_x(y_u,:);
        d_t_A1_u = (-2*cx)./(1+a_x(y_u,1:Nx))./([ones(1,x_c) a_x(y_u,x_c+1:Nx)]);
        d_b_A1_u = (-2*cx)./(1+a_x(y_u,2:Nx+1))./([a_x(y_u,2:nw_half) ones(1,x_c)]);

        bc_t_A1_u(1:nw_half) = C1_x(y_u,1:nw_half).*R_x2_c(y_u,1:nw_half);
        bc_t_A1_u(Nx+2:Nx+x_c) = C3_x(y_u,1:nw_half).*R_x2_c(y_u,1:nw_half);
        bc_b_A1_u(x_c:Nx) = C1_x(y_u,x_c+1:Nx+1).*R_x2_c(y_u,x_c+1:Nx+1);
        bc_b_A1_u(Nx+x_c+1:2*Nx+1) = C3_x(y_u,x_c+1:Nx+1).*R_x2_c(y_u,x_c+1:Nx+1);

        d_t_xy_A1_u(1:nw_half) = -C2_x(y_u,1:nw_half).*R_x2_c(y_u,1:nw_half);
        d_b_xy_A1_u(x_c+1:Nx+1) = -C2_x(y_u,x_c+1:Nx+1).*R_x2_c(y_u,x_c+1:Nx+1);
        d_t_yx_A1_u(2:x_c) = -C4_x(y_u,1:nw_half).*R_x2_c(y_u,1:nw_half);
        d_b_yx_A1_u(x_c:Nx) = -C4_x(y_u,x_c+1:Nx+1).*R_x2_c(y_u,x_c+1:Nx+1);

        d_c_A1_d = 1 + (2*cx)./a_x(y_d,:);
        d_t_A1_d = (-2*cx)./(1+a_x(y_d,1:Nx))./([ones(1,x_c) a_x(y_d,x_c+1:Nx)]);
        d_b_A1_d = (-2*cx)./(1+a_x(y_d,2:Nx+1))./([a_x(y_d,2:nw_half) ones(1,x_c)]);

        bc_t_A1_d(1:nw_half) = C1_x(y_d,1:nw_half).*R_x2_c(y_d,1:nw_half);
        bc_t_A1_d(Nx+2:Nx+x_c) = C3_x(y_d,1:nw_half).*R_x2_c(y_d,1:nw_half);
        bc_b_A1_d(x_c:Nx) = C1_x(y_d,x_c+1:Nx+1).*R_x2_c(y_d,x_c+1:Nx+1);
        bc_b_A1_d(Nx+x_c+1:2*Nx+1) = C3_x(y_d,x_c+1:Nx+1).*R_x2_c(y_d,x_c+1:Nx+1);

        d_t_xy_A1_d(1:nw_half) = -C2_x(y_d,1:nw_half).*R_x2_c(y_d,1:nw_half);
        d_b_xy_A1_d(x_c+1:Nx+1) = -C2_x(y_d,x_c+1:Nx+1).*R_x2_c(y_d,x_c+1:Nx+1);
        d_t_yx_A1_d(2:x_c) = -C4_x(y_d,1:nw_half).*R_x2_c(y_d,1:nw_half);
        d_b_yx_A1_d(x_c:Nx) = -C4_x(y_d,x_c+1:Nx+1).*R_x2_c(y_d,x_c+1:Nx+1);

        A1_u = diag([d_c_A1_u d_c_A1_u], 0) + diag(([d_t_A1_u 0 d_t_A1_u] - bc_t_A1_u), 1) + diag(([d_b_A1_u 0 d_b_A1_u] - bc_b_A1_u), -1)...
               + diag(d_t_xy_A1_u, Nx+2) + diag(d_b_xy_A1_u, Nx) + diag(d_b_yx_A1_u, -Nx-2) + diag(d_t_yx_A1_u, -Nx);
        A1_d = diag([d_c_A1_d d_c_A1_d], 0) + diag(([d_t_A1_d 0 d_t_A1_d] - bc_t_A1_d), 1) + diag(([d_b_A1_d 0 d_b_A1_d] - bc_b_A1_d), -1)...
               + diag(d_t_xy_A1_d, Nx+2) + diag(d_b_xy_A1_d, Nx) + diag(d_b_yx_A1_d, -Nx-2) + diag(d_t_yx_A1_d, -Nx);
        j = x_c;

        bx_1_u(j) = (2*cy)/(1+a_y(y_u,j))/a_y(y_u,j)*Wx(y_u-1,j) + ( 1-(2*cy)/a_y(y_u,j) )*Wx(y_u,j)*R_in_d(y_u,j) + (2*cy)/(1+a_y(y_u,j))*Wx(y_u+1,j)...
                     + R_x2_n(y_u,j)*C1_x(y_u,j)*Wx_m(y_u+1,j) + R_x2_n(y_u,j)*C2_x(y_u,j)*Wy_m(y_u+1,j) ;
        by_1_u(j) = (2*cy)/(1+a_y(y_u,j))/a_y(y_u,j)*Wy(y_u-1,j) + ( 1-(2*cy)/a_y(y_u,j) )*Wy(y_u,j)*R_in_d(y_u,j) + (2*cy)/(1+a_y(y_u,j))*Wy(y_u+1,j)...
                     + R_x2_n(y_u,j)*C3_x(y_u,j)*Wy_m(y_u+1,j) + R_x2_n(y_u,j)*C4_x(y_u,j)*Wx_m(y_u+1,j) ;
        bx_1_d(j) = (2*cy)/(1+a_y(y_d,j))*Wx(y_d-1,j) + ( 1-(2*cy)/a_y(y_d,j) )*Wx(y_d,j)*R_in_d(y_d,j) + (2*cy)/(1+a_y(y_d,j))/a_y(y_d,j)*Wx(y_d+1,j)...
                     + R_x2_n(y_d,j)*C1_x(y_d,j)*Wx_m(y_d-1,j) + R_x2_n(y_d,j)*C2_x(y_d,j)*Wy_m(y_d-1,j) ;
        by_1_d(j) = (2*cy)/(1+a_y(y_d,j))*Wy(y_d-1,j) + ( 1-(2*cy)/a_y(y_d,j) )*Wy(y_d,j)*R_in_d(y_d,j) + (2*cy)/(1+a_y(y_d,j))/a_y(y_d,j)*Wy(y_d+1,j)...
                     + R_x2_n(y_d,j)*C3_x(y_d,j)*Wy_m(y_d-1,j) + R_x2_n(y_d,j)*C4_x(y_d,j)*Wx_m(y_d-1,j) ;
                 
        for j = 1:nw_half
            xx_l = x_c-j;
            xx_r = x_c+j;
            bx_1_u(xx_r) = (2*cy)/(1+a_y(y_u,xx_r))/a_y(y_u,xx_r)*Wx(y_u-1,xx_r) + ( 1-(2*cy)/a_y(y_u,xx_r) )*Wx(y_u,xx_r)*R_in_d(y_u,xx_r) + (2*cy)/(1+a_y(y_u,xx_r))*Wx(y_u+1,xx_r)...
                            + R_x2(y_u,xx_r)*C1_x(y_u,xx_r)*Wx_m(y_u+1,xx_r-1) + R_x2_n(y_u,xx_r)*C1_x(y_u,xx_r)*Wx_m(y_u+1,xx_r)...
                            + R_x2(y_u,xx_r)*C2_x(y_u,xx_r)*Wy_m(y_u+1,xx_r-1) + R_x2_n(y_u,xx_r)*C2_x(y_u,xx_r)*Wy_m(y_u+1,xx_r) ;
            by_1_u(xx_r) = (2*cy)/(1+a_y(y_u,xx_r))/a_y(y_u,xx_r)*Wy(y_u-1,xx_r) + ( 1-(2*cy)/a_y(y_u,xx_r) )*Wy(y_u,xx_r)*R_in_d(y_u,xx_r) + (2*cy)/(1+a_y(y_u,xx_r))*Wy(y_u+1,xx_r)...
                            + R_x2(y_u,xx_r)*C3_x(y_u,xx_r)*Wy_m(y_u+1,xx_r-1) + R_x2_n(y_u,xx_r)*C3_x(y_u,xx_r)*Wy_m(y_u+1,xx_r)...
                            + R_x2(y_u,xx_r)*C4_x(y_u,xx_r)*Wx_m(y_u+1,xx_r-1) + R_x2_n(y_u,xx_r)*C4_x(y_u,xx_r)*Wx_m(y_u+1,xx_r) ;
            bx_1_u(xx_l) = (2*cy)/(1+a_y(y_u,xx_l))/a_y(y_u,xx_l)*Wx(y_u-1,xx_l) + ( 1-(2*cy)/a_y(y_u,xx_l) )*Wx(y_u,xx_l)*R_in_d(y_u,xx_l) + (2*cy)/(1+a_y(y_u,xx_l))*Wx(y_u+1,xx_l)...
                            + R_x2(y_u,xx_l)*C1_x(y_u,xx_l)*Wx_m(y_u+1,xx_l+1) + R_x2_n(y_u,xx_l)*C1_x(y_u,xx_l)*Wx_m(y_u+1,xx_l)...
                            + R_x2(y_u,xx_l)*C2_x(y_u,xx_l)*Wy_m(y_u+1,xx_l+1) + R_x2_n(y_u,xx_l)*C2_x(y_u,xx_l)*Wy_m(y_u+1,xx_l) ;
            by_1_u(xx_l) = (2*cy)/(1+a_y(y_u,xx_l))/a_y(y_u,xx_l)*Wy(y_u-1,xx_l) + ( 1-(2*cy)/a_y(y_u,xx_l) )*Wy(y_u,xx_l)*R_in_d(y_u,xx_l) + (2*cy)/(1+a_y(y_u,xx_l))*Wy(y_u+1,xx_l)...
                            + R_x2(y_u,xx_l)*C3_x(y_u,xx_l)*Wy_m(y_u+1,xx_l+1) + R_x2_n(y_u,xx_l)*C3_x(y_u,xx_l)*Wy_m(y_u+1,xx_l)...
                            + R_x2(y_u,xx_l)*C4_x(y_u,xx_l)*Wx_m(y_u+1,xx_l+1) + R_x2_n(y_u,xx_l)*C4_x(y_u,xx_l)*Wx_m(y_u+1,xx_l) ;
            bx_1_d(xx_r) = (2*cy)/(1+a_y(y_d,xx_r))*Wx(y_d-1,xx_r) + ( 1-(2*cy)/a_y(y_d,xx_r) )*Wx(y_d,xx_r)*R_in_d(y_d,xx_r) + (2*cy)/(1+a_y(y_d,xx_r))/a_y(y_d,xx_r)*Wx(y_d+1,xx_r)...
                            + R_x2(y_d,xx_r)*C1_x(y_d,xx_r)*Wx_m(y_d-1,xx_r-1) + R_x2_n(y_d,xx_r)*C1_x(y_d,xx_r)*Wx_m(y_d-1,xx_r)...
                            + R_x2(y_d,xx_r)*C2_x(y_d,xx_r)*Wy_m(y_d-1,xx_r-1) + R_x2_n(y_d,xx_r)*C2_x(y_d,xx_r)*Wy_m(y_d-1,xx_r) ;
            by_1_d(xx_r) = (2*cy)/(1+a_y(y_d,xx_r))*Wy(y_d-1,xx_r) + ( 1-(2*cy)/a_y(y_d,xx_r) )*Wy(y_d,xx_r)*R_in_d(y_d,xx_r) + (2*cy)/(1+a_y(y_d,xx_r))/a_y(y_d,xx_r)*Wy(y_d+1,xx_r)...
                            + R_x2(y_d,xx_r)*C3_x(y_d,xx_r)*Wy_m(y_d-1,xx_r-1) + R_x2_n(y_d,xx_r)*C3_x(y_d,xx_r)*Wy_m(y_d-1,xx_r)...
                            + R_x2(y_d,xx_r)*C4_x(y_d,xx_r)*Wx_m(y_d-1,xx_r-1) + R_x2_n(y_d,xx_r)*C4_x(y_d,xx_r)*Wx_m(y_d-1,xx_r) ;
            bx_1_d(xx_l) = (2*cy)/(1+a_y(y_d,xx_l))*Wx(y_d-1,xx_l) + ( 1-(2*cy)/a_y(y_d,xx_l) )*Wx(y_d,xx_l)*R_in_d(y_d,xx_l) + (2*cy)/(1+a_y(y_d,xx_l))/a_y(y_d,xx_l)*Wx(y_d+1,xx_l)...
                            + R_x2(y_d,xx_l)*C1_x(y_d,xx_l)*Wx_m(y_d-1,xx_l+1) + R_x2_n(y_d,xx_l)*C1_x(y_d,xx_l)*Wx_m(y_d-1,xx_l)...
                            + R_x2(y_d,xx_l)*C2_x(y_d,xx_l)*Wy_m(y_d-1,xx_l+1) + R_x2_n(y_d,xx_l)*C2_x(y_d,xx_l)*Wy_m(y_d-1,xx_l) ;
            by_1_d(xx_l) = (2*cy)/(1+a_y(y_d,xx_l))*Wy(y_d-1,xx_l) + ( 1-(2*cy)/a_y(y_d,xx_l) )*Wy(y_d,xx_l)*R_in_d(y_d,xx_l) + (2*cy)/(1+a_y(y_d,xx_l))/a_y(y_d,xx_l)*Wy(y_d+1,xx_l)...
                            + R_x2(y_d,xx_l)*C3_x(y_d,xx_l)*Wy_m(y_d-1,xx_l+1) + R_x2_n(y_d,xx_l)*C3_x(y_d,xx_l)*Wy_m(y_d-1,xx_l)...
                            + R_x2(y_d,xx_l)*C4_x(y_d,xx_l)*Wx_m(y_d-1,xx_l+1) + R_x2_n(y_d,xx_l)*C4_x(y_d,xx_l)*Wx_m(y_d-1,xx_l) ;
        end
        
        w1_u = A1_u\( [bx_1_u ; by_1_u] );
        w1_d = A1_d\( [bx_1_d ; by_1_d] );
        Wx_m(y_u,:) = w1_u(1:Nx+1).';
        Wx_m(y_d,:) = w1_d(1:Nx+1).';
        Wy_m(y_u,:) = w1_u(Nx+2:end).';
        Wy_m(y_d,:) = w1_d(Nx+2:end).';    
    end
    
    for i = nh_bottom:nh_top-1
        y_u = y_c-i ;
        d_c_A1_u = 1 + (2*cx)./a_x(y_u,:);
        d_t_A1_u = (-2*cx)./(1+a_x(y_u,1:Nx))./([ones(1,x_c) a_x(y_u,x_c+1:Nx)]);
        d_b_A1_u = (-2*cx)./(1+a_x(y_u,2:Nx+1))./([a_x(y_u,2:nw_half) ones(1,x_c)]);
        bc_t_A1_u(1:nw_half) = C1_x(y_u,1:nw_half).*R_x2_c(y_u,1:nw_half);
        bc_t_A1_u(Nx+2:Nx+x_c) = C3_x(y_u,1:nw_half).*R_x2_c(y_u,1:nw_half);
        bc_b_A1_u(x_c:Nx) = C1_x(y_u,x_c+1:Nx+1).*R_x2_c(y_u,x_c+1:Nx+1);
        bc_b_A1_u(Nx+x_c+1:2*Nx+1) = C3_x(y_u,x_c+1:Nx+1).*R_x2_c(y_u,x_c+1:Nx+1);
        d_t_xy_A1_u(1:nw_half) = -C2_x(y_u,1:nw_half).*R_x2_c(y_u,1:nw_half);
        d_b_xy_A1_u(x_c+1:Nx+1) = -C2_x(y_u,x_c+1:Nx+1).*R_x2_c(y_u,x_c+1:Nx+1);
        d_t_yx_A1_u(2:x_c) = -C4_x(y_u,1:nw_half).*R_x2_c(y_u,1:nw_half);
        d_b_yx_A1_u(x_c:Nx) = -C4_x(y_u,x_c+1:Nx+1).*R_x2_c(y_u,x_c+1:Nx+1);
        A1_u = diag([d_c_A1_u d_c_A1_u], 0) + diag(([d_t_A1_u 0 d_t_A1_u] - bc_t_A1_u), 1) + diag(([d_b_A1_u 0 d_b_A1_u] - bc_b_A1_u), -1)...
               + diag(d_t_xy_A1_u, Nx+2) + diag(d_b_xy_A1_u, Nx) + diag(d_b_yx_A1_u, -Nx-2) + diag(d_t_yx_A1_u, -Nx);
        j = x_c;
        bx_1_u(j) = (2*cy)/(1+a_y(y_u,j))/a_y(y_u,j)*Wx(y_u-1,j) + ( 1-(2*cy)/a_y(y_u,j) )*Wx(y_u,j)*R_in_d(y_u,j) + (2*cy)/(1+a_y(y_u,j))*Wx(y_u+1,j)...
                     + R_x2_n(y_u,j)*C1_x(y_u,j)*Wx_m(y_u+1,j) + R_x2_n(y_u,j)*C2_x(y_u,j)*Wy_m(y_u+1,j) ;
        by_1_u(j) = (2*cy)/(1+a_y(y_u,j))/a_y(y_u,j)*Wy(y_u-1,j) + ( 1-(2*cy)/a_y(y_u,j) )*Wy(y_u,j)*R_in_d(y_u,j) + (2*cy)/(1+a_y(y_u,j))*Wy(y_u+1,j)...
                     + R_x2_n(y_u,j)*C3_x(y_u,j)*Wy_m(y_u+1,j) + R_x2_n(y_u,j)*C4_x(y_u,j)*Wx_m(y_u+1,j) ;
                 
        for j = 1:nw_half
            xx_l = x_c-j;
            xx_r = x_c+j;
            bx_1_u(xx_r) = (2*cy)/(1+a_y(y_u,xx_r))/a_y(y_u,xx_r)*Wx(y_u-1,xx_r) + ( 1-(2*cy)/a_y(y_u,xx_r) )*Wx(y_u,xx_r)*R_in_d(y_u,xx_r) + (2*cy)/(1+a_y(y_u,xx_r))*Wx(y_u+1,xx_r)...
                            + R_x2(y_u,xx_r)*C1_x(y_u,xx_r)*Wx_m(y_u+1,xx_r-1) + R_x2_n(y_u,xx_r)*C1_x(y_u,xx_r)*Wx_m(y_u+1,xx_r)...
                            + R_x2(y_u,xx_r)*C2_x(y_u,xx_r)*Wy_m(y_u+1,xx_r-1) + R_x2_n(y_u,xx_r)*C2_x(y_u,xx_r)*Wy_m(y_u+1,xx_r) ;
            by_1_u(xx_r) = (2*cy)/(1+a_y(y_u,xx_r))/a_y(y_u,xx_r)*Wy(y_u-1,xx_r) + ( 1-(2*cy)/a_y(y_u,xx_r) )*Wy(y_u,xx_r)*R_in_d(y_u,xx_r) + (2*cy)/(1+a_y(y_u,xx_r))*Wy(y_u+1,xx_r)...
                            + R_x2(y_u,xx_r)*C3_x(y_u,xx_r)*Wy_m(y_u+1,xx_r-1) + R_x2_n(y_u,xx_r)*C3_x(y_u,xx_r)*Wy_m(y_u+1,xx_r)...
                            + R_x2(y_u,xx_r)*C4_x(y_u,xx_r)*Wx_m(y_u+1,xx_r-1) + R_x2_n(y_u,xx_r)*C4_x(y_u,xx_r)*Wx_m(y_u+1,xx_r) ;
            bx_1_u(xx_l) = (2*cy)/(1+a_y(y_u,xx_l))/a_y(y_u,xx_l)*Wx(y_u-1,xx_l) + ( 1-(2*cy)/a_y(y_u,xx_l) )*Wx(y_u,xx_l)*R_in_d(y_u,xx_l) + (2*cy)/(1+a_y(y_u,xx_l))*Wx(y_u+1,xx_l)...
                            + R_x2(y_u,xx_l)*C1_x(y_u,xx_l)*Wx_m(y_u+1,xx_l+1) + R_x2_n(y_u,xx_l)*C1_x(y_u,xx_l)*Wx_m(y_u+1,xx_l)...
                            + R_x2(y_u,xx_l)*C2_x(y_u,xx_l)*Wy_m(y_u+1,xx_l+1) + R_x2_n(y_u,xx_l)*C2_x(y_u,xx_l)*Wy_m(y_u+1,xx_l) ;
            by_1_u(xx_l) = (2*cy)/(1+a_y(y_u,xx_l))/a_y(y_u,xx_l)*Wy(y_u-1,xx_l) + ( 1-(2*cy)/a_y(y_u,xx_l) )*Wy(y_u,xx_l)*R_in_d(y_u,xx_l) + (2*cy)/(1+a_y(y_u,xx_l))*Wy(y_u+1,xx_l)...
                            + R_x2(y_u,xx_l)*C3_x(y_u,xx_l)*Wy_m(y_u+1,xx_l+1) + R_x2_n(y_u,xx_l)*C3_x(y_u,xx_l)*Wy_m(y_u+1,xx_l)...
                            + R_x2(y_u,xx_l)*C4_x(y_u,xx_l)*Wx_m(y_u+1,xx_l+1) + R_x2_n(y_u,xx_l)*C4_x(y_u,xx_l)*Wx_m(y_u+1,xx_l) ;
        end
        
        w1_u = A1_u\( [bx_1_u ; by_1_u] );
        Wx_m(y_u,:) = w1_u(1:Nx+1).';
        Wy_m(y_u,:) = w1_u(Nx+2:end).';
    end

    for i = nh_top:nh_bottom-1
        y_d = y_c+i ;
        d_c_A1_d = 1 + (2*cx)./a_x(y_d,:);
        d_t_A1_d = (-2*cx)./(1+a_x(y_d,1:Nx))./([ones(1,x_c) a_x(y_d,x_c+1:Nx)]);
        d_b_A1_d = (-2*cx)./(1+a_x(y_d,2:Nx+1))./([a_x(y_d,2:nw_half) ones(1,x_c)]);
        bc_t_A1_d(1:nw_half) = C1_x(y_d,1:nw_half).*R_x2_c(y_d,1:nw_half);
        bc_t_A1_d(Nx+2:Nx+x_c) = C3_x(y_d,1:nw_half).*R_x2_c(y_d,1:nw_half);
        bc_b_A1_d(x_c:Nx) = C1_x(y_d,x_c+1:Nx+1).*R_x2_c(y_d,x_c+1:Nx+1);
        bc_b_A1_d(Nx+x_c+1:2*Nx+1) = C3_x(y_d,x_c+1:Nx+1).*R_x2_c(y_d,x_c+1:Nx+1);
        d_t_xy_A1_d(1:nw_half) = -C2_x(y_d,1:nw_half).*R_x2_c(y_d,1:nw_half);
        d_b_xy_A1_d(x_c+1:Nx+1) = -C2_x(y_d,x_c+1:Nx+1).*R_x2_c(y_d,x_c+1:Nx+1);
        d_t_yx_A1_d(2:x_c) = -C4_x(y_d,1:nw_half).*R_x2_c(y_d,1:nw_half);
        d_b_yx_A1_d(x_c:Nx) = -C4_x(y_d,x_c+1:Nx+1).*R_x2_c(y_d,x_c+1:Nx+1);
        A1_d = diag([d_c_A1_d d_c_A1_d], 0) + diag(([d_t_A1_d 0 d_t_A1_d] - bc_t_A1_d), 1) + diag(([d_b_A1_d 0 d_b_A1_d] - bc_b_A1_d), -1)...
               + diag(d_t_xy_A1_d, Nx+2) + diag(d_b_xy_A1_d, Nx) + diag(d_b_yx_A1_d, -Nx-2) + diag(d_t_yx_A1_d, -Nx);
        j = x_c;
        bx_1_d(j) = (2*cy)/(1+a_y(y_d,j))*Wx(y_d-1,j) + ( 1-(2*cy)/a_y(y_d,j) )*Wx(y_d,j)*R_in_d(y_d,j) + (2*cy)/(1+a_y(y_d,j))/a_y(y_d,j)*Wx(y_d+1,j)...
                     + R_x2_n(y_d,j)*C1_x(y_d,j)*Wx_m(y_d-1,j) + R_x2_n(y_d,j)*C2_x(y_d,j)*Wy_m(y_d-1,j) ;
        by_1_d(j) = (2*cy)/(1+a_y(y_d,j))*Wy(y_d-1,j) + ( 1-(2*cy)/a_y(y_d,j) )*Wy(y_d,j)*R_in_d(y_d,j) + (2*cy)/(1+a_y(y_d,j))/a_y(y_d,j)*Wy(y_d+1,j)...
                     + R_x2_n(y_d,j)*C3_x(y_d,j)*Wy_m(y_d-1,j) + R_x2_n(y_d,j)*C4_x(y_d,j)*Wx_m(y_d-1,j) ;
                 
        for j = 1:nw_half
            xx_l = x_c-j;
            xx_r = x_c+j;
            bx_1_d(xx_r) = (2*cy)/(1+a_y(y_d,xx_r))*Wx(y_d-1,xx_r) + ( 1-(2*cy)/a_y(y_d,xx_r) )*Wx(y_d,xx_r)*R_in_d(y_d,xx_r) + (2*cy)/(1+a_y(y_d,xx_r))/a_y(y_d,xx_r)*Wx(y_d+1,xx_r)...
                            + R_x2(y_d,xx_r)*C1_x(y_d,xx_r)*Wx_m(y_d-1,xx_r-1) + R_x2_n(y_d,xx_r)*C1_x(y_d,xx_r)*Wx_m(y_d-1,xx_r)...
                            + R_x2(y_d,xx_r)*C2_x(y_d,xx_r)*Wy_m(y_d-1,xx_r-1) + R_x2_n(y_d,xx_r)*C2_x(y_d,xx_r)*Wy_m(y_d-1,xx_r) ;
            by_1_d(xx_r) = (2*cy)/(1+a_y(y_d,xx_r))*Wy(y_d-1,xx_r) + ( 1-(2*cy)/a_y(y_d,xx_r) )*Wy(y_d,xx_r)*R_in_d(y_d,xx_r) + (2*cy)/(1+a_y(y_d,xx_r))/a_y(y_d,xx_r)*Wy(y_d+1,xx_r)...
                            + R_x2(y_d,xx_r)*C3_x(y_d,xx_r)*Wy_m(y_d-1,xx_r-1) + R_x2_n(y_d,xx_r)*C3_x(y_d,xx_r)*Wy_m(y_d-1,xx_r)...
                            + R_x2(y_d,xx_r)*C4_x(y_d,xx_r)*Wx_m(y_d-1,xx_r-1) + R_x2_n(y_d,xx_r)*C4_x(y_d,xx_r)*Wx_m(y_d-1,xx_r) ;
            bx_1_d(xx_l) = (2*cy)/(1+a_y(y_d,xx_l))*Wx(y_d-1,xx_l) + ( 1-(2*cy)/a_y(y_d,xx_l) )*Wx(y_d,xx_l)*R_in_d(y_d,xx_l) + (2*cy)/(1+a_y(y_d,xx_l))/a_y(y_d,xx_l)*Wx(y_d+1,xx_l)...
                            + R_x2(y_d,xx_l)*C1_x(y_d,xx_l)*Wx_m(y_d-1,xx_l+1) + R_x2_n(y_d,xx_l)*C1_x(y_d,xx_l)*Wx_m(y_d-1,xx_l)...
                            + R_x2(y_d,xx_l)*C2_x(y_d,xx_l)*Wy_m(y_d-1,xx_l+1) + R_x2_n(y_d,xx_l)*C2_x(y_d,xx_l)*Wy_m(y_d-1,xx_l) ;
            by_1_d(xx_l) = (2*cy)/(1+a_y(y_d,xx_l))*Wy(y_d-1,xx_l) + ( 1-(2*cy)/a_y(y_d,xx_l) )*Wy(y_d,xx_l)*R_in_d(y_d,xx_l) + (2*cy)/(1+a_y(y_d,xx_l))/a_y(y_d,xx_l)*Wy(y_d+1,xx_l)...
                            + R_x2(y_d,xx_l)*C3_x(y_d,xx_l)*Wy_m(y_d-1,xx_l+1) + R_x2_n(y_d,xx_l)*C3_x(y_d,xx_l)*Wy_m(y_d-1,xx_l)...
                            + R_x2(y_d,xx_l)*C4_x(y_d,xx_l)*Wx_m(y_d-1,xx_l+1) + R_x2_n(y_d,xx_l)*C4_x(y_d,xx_l)*Wx_m(y_d-1,xx_l) ;
        end
     
        w1_d = A1_d\( [bx_1_d ; by_1_d] );
        Wx_m(y_d,:) = w1_d(1:Nx+1).';
        Wy_m(y_d,:) = w1_d(Nx+2:end).';    
    
    end
    d_c_A2 = 1 + (2*cy)./a_y(:,x_c);
    d_t_A2 = (-2*cy)./(1+a_y(1:Ny,x_c))./([ones(y_c,1); a_y(y_c+1:Ny,x_c)]);
    d_b_A2 = (-2*cy)./(1+a_y(2:Ny+1,x_c))./([a_y(2:nh_top,x_c); ones(nh_bottom+1,1)]);
    bc_t_A2(1:nh_top) = C1_y(1:nh_top,x_c).*R_y1_c(1:nh_top,x_c);
    bc_t_A2(Ny+2:Ny+y_c) = C3_y(1:nh_top,x_c).*R_y1_c(1:nh_top,x_c);
    bc_b_A2(y_c:Ny) = C1_y(y_c+1:Ny+1,x_c).*R_y1_c(y_c+1:Ny+1,x_c);
    bc_b_A2(Ny+y_c+1:2*Ny+1) = C3_y(y_c+1:Ny+1,x_c).*R_y1_c(y_c+1:Ny+1,x_c);
    d_t_xy_A2(1:nh_top) = -C2_y(1:nh_top,x_c).*R_y1_c(1:nh_top,x_c);
    d_b_xy_A2(y_c+1:Ny+1) = -C2_y(y_c+1:Ny+1,x_c).*R_y1_c(y_c+1:Ny+1,x_c);
    d_t_yx_A2(2:y_c) = -C4_y(1:nh_top,x_c).*R_y1_c(1:nh_top,x_c);
    d_b_yx_A2(y_c:Ny) = -C4_y(y_c+1:Ny+1,x_c).*R_y1_c(y_c+1:Ny+1,x_c);
    A2_c = diag([d_c_A2; d_c_A2], 0) + diag(([d_t_A2; 0; d_t_A2] - bc_t_A2), 1) + diag(([d_b_A2; 0; d_b_A2] - bc_b_A2), -1)...
           + diag(d_t_xy_A2, Ny+2) + diag(d_b_xy_A2, Ny) + diag(d_b_yx_A2, -Ny-2) + diag(d_t_yx_A2, -Ny);

    for ii = 2:Ny
        bx_2(ii) = cx*Wx_m(ii,x_c-1) + (1-2*cx)*Wx_m(ii,x_c) + cx*Wx_m(ii,x_c+1);
        by_2(ii) = cx*Wy_m(ii,x_c-1) + (1-2*cx)*Wy_m(ii,x_c) + cx*Wy_m(ii,x_c+1);
    end
    w2_c = A2_c\( [ bx_2.*R_in_d(:,x_c) ; by_2.*R_in_d(:,x_c) ] );
    Wx(:,x_c) = w2_c(1:Ny+1);
    Wy(:,x_c) = w2_c(Ny+2:end);
%1111111
    for jj = 1:nw_half-1
        x_l = x_c-jj;
        x_r = x_c+jj;
        d_c_A2_l = 1 + (2*cy)./a_y(:,x_l);
        d_t_A2_l = (-2*cy)./(1+a_y(1:Ny,x_l))./([ones(y_c,1); a_y(y_c+1:Ny,x_l)]);
        d_b_A2_l = (-2*cy)./(1+a_y(2:Ny+1,x_l))./([a_y(2:nh_top,x_l); ones(nh_bottom+1,1)]);
        bc_t_A2_l(1:nh_top) = C1_y(1:nh_top,x_l).*R_y1_c(1:nh_top,x_l);
        bc_t_A2_l(Ny+2:Ny+y_c) = C3_y(1:nh_top,x_l).*R_y1_c(1:nh_top,x_l);
        bc_b_A2_l(y_c:Ny) = C1_y(y_c+1:Ny+1,x_l).*R_y1_c(y_c+1:Ny+1,x_l);
        bc_b_A2_l(Ny+y_c+1:2*Ny+1) = C3_y(y_c+1:Ny+1,x_l).*R_y1_c(y_c+1:Ny+1,x_l);
        d_t_xy_A2_l(1:nh_top) = -C2_y(1:nh_top,x_l).*R_y1_c(1:nh_top,x_l);
        d_b_xy_A2_l(y_c+1:Ny+1) = -C2_y(y_c+1:Ny+1,x_l).*R_y1_c(y_c+1:Ny+1,x_l);
        d_t_yx_A2_l(2:y_c) = -C4_y(1:nh_top,x_l).*R_y1_c(1:nh_top,x_l);
        d_b_yx_A2_l(y_c:Ny) = -C4_y(y_c+1:Ny+1,x_l).*R_y1_c(y_c+1:Ny+1,x_l);
        d_c_A2_r = 1 + (2*cy)./a_y(:,x_r);
        d_t_A2_r = (-2*cy)./(1+a_y(1:Ny,x_r))./([ones(y_c,1); a_y(y_c+1:Ny,x_r)]);
        d_b_A2_r = (-2*cy)./(1+a_y(2:Ny+1,x_r))./([a_y(2:nh_top,x_r); ones(nh_bottom+1,1)]);
        bc_t_A2_r(1:nh_top) = C1_y(1:nh_top,x_r).*R_y1_c(1:nh_top,x_r);
        bc_t_A2_r(Ny+2:Ny+y_c) = C3_y(1:nh_top,x_r).*R_y1_c(1:nh_top,x_r);
        bc_b_A2_r(y_c:Ny) = C1_y(y_c+1:Ny+1,x_r).*R_y1_c(y_c+1:Ny+1,x_r);
        bc_b_A2_r(Ny+y_c+1:2*Ny+1) = C3_y(y_c+1:Ny+1,x_r).*R_y1_c(y_c+1:Ny+1,x_r);
        d_t_xy_A2_r(1:nh_top) = -C2_y(1:nh_top,x_r).*R_y1_c(1:nh_top,x_r);
        d_b_xy_A2_r(y_c+1:Ny+1) = -C2_y(y_c+1:Ny+1,x_r).*R_y1_c(y_c+1:Ny+1,x_r);
        d_t_yx_A2_r(2:y_c) = -C4_y(1:nh_top,x_r).*R_y1_c(1:nh_top,x_r);
        d_b_yx_A2_r(y_c:Ny) = -C4_y(y_c+1:Ny+1,x_r).*R_y1_c(y_c+1:Ny+1,x_r);
        A2_l = diag([d_c_A2_l; d_c_A2_l], 0) + diag(([d_t_A2_l; 0; d_t_A2_l] - bc_t_A2_l), 1) + diag(([d_b_A2_l; 0; d_b_A2_l] - bc_b_A2_l), -1)...
               + diag(d_t_xy_A2_l, Ny+2) + diag(d_b_xy_A2_l, Ny) + diag(d_b_yx_A2_l, -Ny-2) + diag(d_t_yx_A2_l, -Ny);
        A2_r = diag([d_c_A2_r; d_c_A2_r], 0) + diag(([d_t_A2_r; 0; d_t_A2_r] - bc_t_A2_r), 1) + diag(([d_b_A2_r; 0; d_b_A2_r] - bc_b_A2_r), -1)...
               + diag(d_t_xy_A2_r, Ny+2) + diag(d_b_xy_A2_r, Ny) + diag(d_b_yx_A2_r, -Ny-2) + diag(d_t_yx_A2_r, -Ny);
        ii = y_c;
        bx_2_l(ii) = (2*cx)/(1+a_x(ii,x_l))/a_x(ii,x_l)*Wx_m(ii,x_l-1) + ( 1-(2*cx)/a_x(ii,x_l) )*Wx_m(ii,x_l)*R_in_d(ii,x_l) + (2*cx)/(1+a_x(ii,x_l))*Wx_m(ii,x_l+1)...
                     + R_y1_n(ii,x_l)*C1_y(ii,x_l)*Wx(ii,x_l+1) + R_y1_n(ii,x_l)*C2_y(ii,x_l)*Wy(ii,x_l+1) ;
        by_2_l(ii) = (2*cx)/(1+a_x(ii,x_l))/a_x(ii,x_l)*Wy_m(ii,x_l-1) + ( 1-(2*cx)/a_x(ii,x_l) )*Wy_m(ii,x_l)*R_in_d(ii,x_l) + (2*cx)/(1+a_x(ii,x_l))*Wy_m(ii,x_l+1)...
                     + R_y1_n(ii,x_l)*C3_y(ii,x_l)*Wy(ii,x_l+1) + R_y1_n(ii,x_l)*C4_y(ii,x_l)*Wx(ii,x_l+1) ;
        bx_2_r(ii) = (2*cx)/(1+a_x(ii,x_r))*Wx_m(ii,x_r-1) + ( 1-(2*cx)/a_x(ii,x_r) )*Wx_m(ii,x_r)*R_in_d(ii,x_r) + (2*cx)/(1+a_x(ii,x_r))/a_x(ii,x_r)*Wx_m(ii,x_r+1)...
                     + R_y1_n(ii,x_r)*C1_y(ii,x_r)*Wx(ii,x_r-1) + R_y1_n(ii,x_r)*C2_y(ii,x_r)*Wy(ii,x_r-1) ;
        by_2_r(ii) = (2*cx)/(1+a_x(ii,x_r))*Wy_m(ii,x_r-1) + ( 1-(2*cx)/a_x(ii,x_r) )*Wy_m(ii,x_r)*R_in_d(ii,x_r) + (2*cx)/(1+a_x(ii,x_r))/a_x(ii,x_r)*Wy_m(ii,x_r+1)...
                     + R_y1_n(ii,x_r)*C3_y(ii,x_r)*Wy(ii,x_r-1) + R_y1_n(ii,x_r)*C4_y(ii,x_r)*Wx(ii,x_r-1) ;
                 
        for ii = 1:min(nh_bottom, nh_top)
            yy_t = y_c-ii;
            yy_b = y_c+ii;
            bx_2_l(yy_b) = (2*cx)/(1+a_x(yy_b,x_l))/a_x(yy_b,x_l)*Wx_m(yy_b,x_l-1) + ( 1-(2*cx)/a_x(yy_b,x_l) )*Wx_m(yy_b,x_l)*R_in_d(yy_b,x_l) + (2*cx)/(1+a_x(yy_b,x_l))*Wx_m(yy_b,x_l+1)...
                            + R_y1(yy_b,x_l)*C1_y(yy_b,x_l)*Wx(yy_b-1,x_l+1) + R_y1_n(yy_b,x_l)*C1_y(yy_b,x_l)*Wx(yy_b,x_l+1)...
                            + R_y1(yy_b,x_l)*C2_y(yy_b,x_l)*Wy(yy_b-1,x_l+1) + R_y1_n(yy_b,x_l)*C2_y(yy_b,x_l)*Wy(yy_b,x_l+1) ;
            by_2_l(yy_b) = (2*cx)/(1+a_x(yy_b,x_l))/a_x(yy_b,x_l)*Wy_m(yy_b,x_l-1) + ( 1-(2*cx)/a_x(yy_b,x_l) )*Wy_m(yy_b,x_l)*R_in_d(yy_b,x_l) + (2*cx)/(1+a_x(yy_b,x_l))*Wy_m(yy_b,x_l+1)...
                            + R_y1(yy_b,x_l)*C3_y(yy_b,x_l)*Wy(yy_b-1,x_l+1) + R_y1_n(yy_b,x_l)*C3_y(yy_b,x_l)*Wy(yy_b,x_l+1)...
                            + R_y1(yy_b,x_l)*C4_y(yy_b,x_l)*Wx(yy_b-1,x_l+1) + R_y1_n(yy_b,x_l)*C4_y(yy_b,x_l)*Wx(yy_b,x_l+1) ;
            bx_2_l(yy_t) = (2*cx)/(1+a_x(yy_t,x_l))/a_x(yy_t,x_l)*Wx_m(yy_t,x_l-1) + ( 1-(2*cx)/a_x(yy_t,x_l) )*Wx_m(yy_t,x_l)*R_in_d(yy_t,x_l) + (2*cx)/(1+a_x(yy_t,x_l))*Wx_m(yy_t,x_l+1)...
                            + R_y1(yy_t,x_l)*C1_y(yy_t,x_l)*Wx(yy_t+1,x_l+1) + R_y1_n(yy_t,x_l)*C1_y(yy_t,x_l)*Wx(yy_t,x_l+1)...
                            + R_y1(yy_t,x_l)*C2_y(yy_t,x_l)*Wy(yy_t+1,x_l+1) + R_y1_n(yy_t,x_l)*C2_y(yy_t,x_l)*Wy(yy_t,x_l+1) ;
            by_2_l(yy_t) = (2*cx)/(1+a_x(yy_t,x_l))/a_x(yy_t,x_l)*Wy_m(yy_t,x_l-1) + ( 1-(2*cx)/a_x(yy_t,x_l) )*Wy_m(yy_t,x_l)*R_in_d(yy_t,x_l) + (2*cx)/(1+a_x(yy_t,x_l))*Wy_m(yy_t,x_l+1)...
                            + R_y1(yy_t,x_l)*C3_y(yy_t,x_l)*Wy(yy_t+1,x_l+1) + R_y1_n(yy_t,x_l)*C3_y(yy_t,x_l)*Wy(yy_t,x_l+1)...
                            + R_y1(yy_t,x_l)*C4_y(yy_t,x_l)*Wx(yy_t+1,x_l+1) + R_y1_n(yy_t,x_l)*C4_y(yy_t,x_l)*Wx(yy_t,x_l+1) ;
            bx_2_r(yy_b) = (2*cx)/(1+a_x(yy_b,x_r))*Wx_m(yy_b,x_r-1) + ( 1-(2*cx)/a_x(yy_b,x_r) )*Wx_m(yy_b,x_r)*R_in_d(yy_b,x_r) + (2*cx)/(1+a_x(yy_b,x_r))/a_x(yy_b,x_r)*Wx_m(yy_b,x_r+1)...
                            + R_y1(yy_b,x_r)*C1_y(yy_b,x_r)*Wx(yy_b-1,x_r-1) + R_y1_n(yy_b,x_r)*C1_y(yy_b,x_r)*Wx(yy_b,x_r-1)...
                            + R_y1(yy_b,x_r)*C2_y(yy_b,x_r)*Wy(yy_b-1,x_r-1) + R_y1_n(yy_b,x_r)*C2_y(yy_b,x_r)*Wy(yy_b,x_r-1) ;
            by_2_r(yy_b) = (2*cx)/(1+a_x(yy_b,x_r))*Wy_m(yy_b,x_r-1) + ( 1-(2*cx)/a_x(yy_b,x_r) )*Wy_m(yy_b,x_r)*R_in_d(yy_b,x_r) + (2*cx)/(1+a_x(yy_b,x_r))/a_x(yy_b,x_r)*Wy_m(yy_b,x_r+1)...
                            + R_y1(yy_b,x_r)*C3_y(yy_b,x_r)*Wy(yy_b-1,x_r-1) + R_y1_n(yy_b,x_r)*C3_y(yy_b,x_r)*Wy(yy_b,x_r-1)...
                            + R_y1(yy_b,x_r)*C4_y(yy_b,x_r)*Wx(yy_b-1,x_r-1) + R_y1_n(yy_b,x_r)*C4_y(yy_b,x_r)*Wx(yy_b,x_r-1) ;
            bx_2_r(yy_t) = (2*cx)/(1+a_x(yy_t,x_r))*Wx_m(yy_t,x_r-1) + ( 1-(2*cx)/a_x(yy_t,x_r) )*Wx_m(yy_t,x_r)*R_in_d(yy_t,x_r) + (2*cx)/(1+a_x(yy_t,x_r))/a_x(yy_t,x_r)*Wx_m(yy_t,x_r+1)...
                            + R_y1(yy_t,x_r)*C1_y(yy_t,x_r)*Wx(yy_t+1,x_r-1) + R_y1_n(yy_t,x_r)*C1_y(yy_t,x_r)*Wx(yy_t,x_r-1)...
                            + R_y1(yy_t,x_r)*C2_y(yy_t,x_r)*Wy(yy_t+1,x_r-1) + R_y1_n(yy_t,x_r)*C2_y(yy_t,x_r)*Wy(yy_t,x_r-1) ;
            by_2_r(yy_t) = (2*cx)/(1+a_x(yy_t,x_r))*Wy_m(yy_t,x_r-1) + ( 1-(2*cx)/a_x(yy_t,x_r) )*Wy_m(yy_t,x_r)*R_in_d(yy_t,x_r) + (2*cx)/(1+a_x(yy_t,x_r))/a_x(yy_t,x_r)*Wy_m(yy_t,x_r+1)...
                            + R_y1(yy_t,x_r)*C3_y(yy_t,x_r)*Wy(yy_t+1,x_r-1) + R_y1_n(yy_t,x_r)*C3_y(yy_t,x_r)*Wy(yy_t,x_r-1)...
                            + R_y1(yy_t,x_r)*C4_y(yy_t,x_r)*Wx(yy_t+1,x_r-1) + R_y1_n(yy_t,x_r)*C4_y(yy_t,x_r)*Wx(yy_t,x_r-1) ;
        end
        
        for ii = nh_bottom+1:nh_top
            yy_t = y_c-ii;
            bx_2_l(yy_t) = (2*cx)/(1+a_x(yy_t,x_l))/a_x(yy_t,x_l)*Wx_m(yy_t,x_l-1) + ( 1-(2*cx)/a_x(yy_t,x_l) )*Wx_m(yy_t,x_l)*R_in_d(yy_t,x_l) + (2*cx)/(1+a_x(yy_t,x_l))*Wx_m(yy_t,x_l+1)...
                            + R_y1(yy_t,x_l)*C1_y(yy_t,x_l)*Wx(yy_t+1,x_l+1) + R_y1_n(yy_t,x_l)*C1_y(yy_t,x_l)*Wx(yy_t,x_l+1)...
                            + R_y1(yy_t,x_l)*C2_y(yy_t,x_l)*Wy(yy_t+1,x_l+1) + R_y1_n(yy_t,x_l)*C2_y(yy_t,x_l)*Wy(yy_t,x_l+1) ;
            by_2_l(yy_t) = (2*cx)/(1+a_x(yy_t,x_l))/a_x(yy_t,x_l)*Wy_m(yy_t,x_l-1) + ( 1-(2*cx)/a_x(yy_t,x_l) )*Wy_m(yy_t,x_l)*R_in_d(yy_t,x_l) + (2*cx)/(1+a_x(yy_t,x_l))*Wy_m(yy_t,x_l+1)...
                            + R_y1(yy_t,x_l)*C3_y(yy_t,x_l)*Wy(yy_t+1,x_l+1) + R_y1_n(yy_t,x_l)*C3_y(yy_t,x_l)*Wy(yy_t,x_l+1)...
                            + R_y1(yy_t,x_l)*C4_y(yy_t,x_l)*Wx(yy_t+1,x_l+1) + R_y1_n(yy_t,x_l)*C4_y(yy_t,x_l)*Wx(yy_t,x_l+1) ;
            bx_2_r(yy_t) = (2*cx)/(1+a_x(yy_t,x_r))*Wx_m(yy_t,x_r-1) + ( 1-(2*cx)/a_x(yy_t,x_r) )*Wx_m(yy_t,x_r)*R_in_d(yy_t,x_r) + (2*cx)/(1+a_x(yy_t,x_r))/a_x(yy_t,x_r)*Wx_m(yy_t,x_r+1)...
                            + R_y1(yy_t,x_r)*C1_y(yy_t,x_r)*Wx(yy_t+1,x_r-1) + R_y1_n(yy_t,x_r)*C1_y(yy_t,x_r)*Wx(yy_t,x_r-1)...
                            + R_y1(yy_t,x_r)*C2_y(yy_t,x_r)*Wy(yy_t+1,x_r-1) + R_y1_n(yy_t,x_r)*C2_y(yy_t,x_r)*Wy(yy_t,x_r-1) ;
            by_2_r(yy_t) = (2*cx)/(1+a_x(yy_t,x_r))*Wy_m(yy_t,x_r-1) + ( 1-(2*cx)/a_x(yy_t,x_r) )*Wy_m(yy_t,x_r)*R_in_d(yy_t,x_r) + (2*cx)/(1+a_x(yy_t,x_r))/a_x(yy_t,x_r)*Wy_m(yy_t,x_r+1)...
                            + R_y1(yy_t,x_r)*C3_y(yy_t,x_r)*Wy(yy_t+1,x_r-1) + R_y1_n(yy_t,x_r)*C3_y(yy_t,x_r)*Wy(yy_t,x_r-1)...
                            + R_y1(yy_t,x_r)*C4_y(yy_t,x_r)*Wx(yy_t+1,x_r-1) + R_y1_n(yy_t,x_r)*C4_y(yy_t,x_r)*Wx(yy_t,x_r-1) ;
        end
        
        for ii = nh_top+1:nh_bottom
            yy_b = y_c+ii;
            bx_2_l(yy_b) = (2*cx)/(1+a_x(yy_b,x_l))/a_x(yy_b,x_l)*Wx_m(yy_b,x_l-1) + ( 1-(2*cx)/a_x(yy_b,x_l) )*Wx_m(yy_b,x_l)*R_in_d(yy_b,x_l) + (2*cx)/(1+a_x(yy_b,x_l))*Wx_m(yy_b,x_l+1)...
                            + R_y1(yy_b,x_l)*C1_y(yy_b,x_l)*Wx(yy_b-1,x_l+1) + R_y1_n(yy_b,x_l)*C1_y(yy_b,x_l)*Wx(yy_b,x_l+1)...
                            + R_y1(yy_b,x_l)*C2_y(yy_b,x_l)*Wy(yy_b-1,x_l+1) + R_y1_n(yy_b,x_l)*C2_y(yy_b,x_l)*Wy(yy_b,x_l+1) ;
            by_2_l(yy_b) = (2*cx)/(1+a_x(yy_b,x_l))/a_x(yy_b,x_l)*Wy_m(yy_b,x_l-1) + ( 1-(2*cx)/a_x(yy_b,x_l) )*Wy_m(yy_b,x_l)*R_in_d(yy_b,x_l) + (2*cx)/(1+a_x(yy_b,x_l))*Wy_m(yy_b,x_l+1)...
                            + R_y1(yy_b,x_l)*C3_y(yy_b,x_l)*Wy(yy_b-1,x_l+1) + R_y1_n(yy_b,x_l)*C3_y(yy_b,x_l)*Wy(yy_b,x_l+1)...
                            + R_y1(yy_b,x_l)*C4_y(yy_b,x_l)*Wx(yy_b-1,x_l+1) + R_y1_n(yy_b,x_l)*C4_y(yy_b,x_l)*Wx(yy_b,x_l+1) ;
            bx_2_r(yy_b) = (2*cx)/(1+a_x(yy_b,x_r))*Wx_m(yy_b,x_r-1) + ( 1-(2*cx)/a_x(yy_b,x_r) )*Wx_m(yy_b,x_r)*R_in_d(yy_b,x_r) + (2*cx)/(1+a_x(yy_b,x_r))/a_x(yy_b,x_r)*Wx_m(yy_b,x_r+1)...
                            + R_y1(yy_b,x_r)*C1_y(yy_b,x_r)*Wx(yy_b-1,x_r-1) + R_y1_n(yy_b,x_r)*C1_y(yy_b,x_r)*Wx(yy_b,x_r-1)...
                            + R_y1(yy_b,x_r)*C2_y(yy_b,x_r)*Wy(yy_b-1,x_r-1) + R_y1_n(yy_b,x_r)*C2_y(yy_b,x_r)*Wy(yy_b,x_r-1) ;
            by_2_r(yy_b) = (2*cx)/(1+a_x(yy_b,x_r))*Wy_m(yy_b,x_r-1) + ( 1-(2*cx)/a_x(yy_b,x_r) )*Wy_m(yy_b,x_r)*R_in_d(yy_b,x_r) + (2*cx)/(1+a_x(yy_b,x_r))/a_x(yy_b,x_r)*Wy_m(yy_b,x_r+1)...
                            + R_y1(yy_b,x_r)*C3_y(yy_b,x_r)*Wy(yy_b-1,x_r-1) + R_y1_n(yy_b,x_r)*C3_y(yy_b,x_r)*Wy(yy_b,x_r-1)...
                            + R_y1(yy_b,x_r)*C4_y(yy_b,x_r)*Wx(yy_b-1,x_r-1) + R_y1_n(yy_b,x_r)*C4_y(yy_b,x_r)*Wx(yy_b,x_r-1) ;
        end
        w2_l = A2_l\( [bx_2_l ; by_2_l] );
        w2_r = A2_r\( [bx_2_r ; by_2_r] );
        Wx(:,x_l) = w2_l(1:Ny+1);
        Wx(:,x_r) = w2_r(1:Ny+1);
        Wy(:,x_l) = w2_l(Ny+2:end);
        Wy(:,x_r) = w2_r(Ny+2:end);    
    end
    Ex = Wx*exp(-1j*k0*(s_start+z*ds));
    Ey = Wy*exp(-1j*k0*(s_start+z*ds));
    Ex_all(:,:,z+1) = Ex;
    Ey_all(:,:,z+1) = Ey;
    
end
end

%%% Self-defined funtion to avoid the floating-points issues (ceil(1.20000/0.05)=25)
function [y] = V_roundn(x)
   v = version;                  % check matlab version
   if str2double(v(end-2)) == 3  % version is Matlab R2013a
       y = roundn(x,-8);         % round 1.20000000000000004 to 1.200000000
   else                          % version is Matlab R2014b
       y = round(x,8);
   end
end